%SerialLink.gravload Gravity load on joints
%
% TAUG = R.gravload(Q) is the joint gravity loading (1xN) for the robot R
% in the joint configuration Q (1xN), where N is the number of robot
% joints.  Gravitational acceleration is a property of the robot object.
%
% If Q is a matrix (MxN) each row is interpreted as a joint configuration
% vector, and the result is a matrix (MxN) each row being the corresponding
% joint torques.
%
% TAUG = R.gravload(Q, GRAV) as above but the gravitational
% acceleration vector GRAV is given explicitly.
%
% See also SerialLink.gravjac, SerialLink.rne, SerialLink.itorque, SerialLink.coriolis.

% 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 tg = gravload(robot, q, grav)
	assert(numcols(q) == robot.n, 'RTB:SerialLink:gravload:badarg', 'Insufficient columns in q');

	if nargin == 2
		tg = rne(robot, q, zeros(size(q)), zeros(size(q)));
	elseif nargin == 3
		tg = rne(robot, q, zeros(size(q)), zeros(size(q)), grav);
    end


function out = colorMatchingFunctions
%colorMatchingFunctions Color-matching functions.
%   colorMatchingFunctions returns a table containing the color matching
%   functions for the CIE 1931 Standard Observer. The table contains the
%   variables lambda, x, y, and z. The color matching functions are
%   returned for wavelengths between 360nm and 830nm at intervals of 1nm.

%   Written by Steve Eddins to accompany Digital Image Processing Using
%   MATLAB, 3rd edition, Gatesmark Press, 2020,
%   http://imageprocessingplace.com.
%
%   Copyright 2019 The MathWorks, Inc.
%   License: https://github.com/mathworks/matlab-color-tools/blob/master/license.txt

persistent t

if isempty(t)
   t = readtable('CIE 1931 Standard Observer (1nm).xlsx',...
      'UseExcel',false);
end

out = t;


function value = month_length_islamic ( y, m )

%*****************************************************************************80
%
%% MONTH_LENGTH_ISLAMIC returns the number of days in an Islamic month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year in which the month occurred.
%
%    Input, integer M, the number of the month.
%
%    Output, integer MONTH_LENGTH_ISLAMIC, the number of days
%    in the month.
%
  mdays = [ 30, 29, 30, 29, 30, 29, 30, 29, 30, 29, 30, 29 ];
%
%  Copy the input.
%
  m2 = m;
  y2 = y;
%
%  Check the input.
%
  [ y2, m2, ierror ] = ym_check_islamic ( y2, m2 );

  if ( ierror ~= 0 )
    month_length_islamic = 0;
    return
  end
%
%  Get the number of days in the month.
%
  value = mdays(m2);
%
%  If necessary, add 1 day for a leap year.
%
  if ( m2 == 12 && year_is_leap_islamic ( y2 ) )
    value = value + 1;
  end

  return
end


function [varargout] = xpower(x,varargin)
% [z80_ind,z80_grp,OUT] = xpower(x,[c],[true],[N],[gsd],[V],[S],[trueX])
% Calculates individual and group power as a function of design matrix,
% true effect magnitudes, and contrasts
% using central limit theorem approximations/theory for estimates
%
% tor wager, 2 / 22 / 04
%
% Defaults
% gsd = .28;          % group standard deviation, relative to true effect;
%                       default from VNL experiment (could be overestimate, bec. btwn + within
%                       not separated)
% N = 10;             % number of subjects, for group stats
% true                true effects; 1 is an increase of 1 std. dev. of scanner noise per event
%                     default is .47 SNR everywhere the first contrast has positive
%                     weights
%                     VNL estimates of signal / noise ratio for visual impulse response, 16 Hz
%                     flashing checkerboard for .5 s
%                     0.83% signal change response, 1.67% signal change error std. dev., .47 SNR
% c = [1 1 -1 -1];    % contrast matrix; one row per contrast, one column
% per condition; default is one contrast for each effect
%
% V                   is noise autocorrelation matrix,
%                       symmetric and ones on the diagonal, n x n (n is number of samples)
%                       default is white noise; see also
%                       canonical_autocorrelation
%
% S                   is filtering (high/low pass) matrix; see getSmoothing
%
%
% see power_map1.m for examples and scripts
% see xzpower for a simulation-based approach
%
% outputs:
% z80_ind and z80_grp are the power estimates for individual and group
% designs.  These correspond to the z-value that is expected to be reached
% in 80% of the realizations of the design.

% Another way of creating the matrices:
%E = inline('inv(x'' * x) * x'' * Vi','x','Vi'); % E is error cov matrix
% matrix of orthogonal projection onto model space * Vi
% test: demo to verify that E is constant for constant SNR, regardless of
% sigma and t (as long as their ratio is equivalent)
% sigma = 1; t = .5; e = E(S*X,S* sigma * Vi);ee = e * e', t ./ trace(ee).^.5

% -----------------------------------------------------------------
% Set up input arguments and defaults
% -----------------------------------------------------------------

% notes: 2/7/07:
% keep true signal |h| = 1, or input as entered
% wsd and gsd are error st. deviations, so SNR = true / wsd (individual
% level) and true / ...

wsd = .47;          % within-subjects d, signal / noise == wsd / 1
gsd = .28;            % group standard deviation, relative to true effect; default from VNL
N = 10;             % number of subjects, for group stats

for i = 1:length(varargin)
    switch i
        case 1
            if isempty(varargin{i})
                c = [eye(size(x,2)-1) zeros(size(x,2)-1,1)];
                % contrast matrix; one row per contrast, one column per condition
            else
                c = varargin{i};
            end
        case 2
            if isempty(varargin{i})
                true = c(1,:) > 0;   % true effects; 1 is an increase of magnitude 1; SNR depends on wsd and gsd
                % % % true = true .* .47;  % estimated SNR from visual experiment (VNL), n = 11
            else
                true = varargin{i};
            end
        case 3
            N = varargin{i};
        case 4
            gsd = varargin{i};
        case 5
            V = varargin{i};
        case 6
            S = varargin{i};
        case 7
            trueX = varargin{i};
    end
end

% defaults, if not entered

if ~(exist('c') == 1) || isempty(c), c = [eye(size(x,2)-1) zeros(size(x,2)-1,1)];   end
if ~(exist('true') == 1) || isempty(true),true = 1 .* (c(1,:) > 0); end

if ~(exist('V') == 1) || isempty(V), V = eye(size(x,1)); xc = [1 0 0];
    %[xc,V] = canonical_autocorrelation
end
if ~(exist('S') == 1), S = []; end

if ~(exist('trueX') == 1) || isempty(trueX), trueX = x; end

% defaults, if empty
%if isempty(true),true = 0.47 .* (c(1,:) > 0);,end
if isempty(N),N = 10; end
if isempty(gsd),gsd = 0.28; end
%if isempty(V), V = eye(size(x,1)); ,xc = [1 0 0];,end

% -----------------------------------------------------------------
% Do the power calculation - start with noncentrality parameters
% -----------------------------------------------------------------

if ~isempty(S),
    if size(S,1) ~= size(S,2), error('S is not square.'), end
    if size(S,1) ~= size(x,1), error('S and X dims do not match.'), end
    if size(S,1) ~= size(V,1), error('S and Vi dims do not match.'), end

    %xo = x;     % save original x, for true signal estimate
    x = S * x; V = S * V * S';  %V = S * V;
end

if size(c,2) < size(x,2), c(:,end+1) = 0;, end
if size(true,2) < size(x,2), true(:,end+1) = 0;, end

c = c';

if ~isempty(S)
    %y = S * x * true'; % S * xorig * true and x * true are equivalent; no need for xo
    % tor modified may 06
    y = S * trueX * true';      % which = S * (xorig*true), filtered orig. data
else
    y = trueX * true';
end

xtxi = inv(x' * x);
px = xtxi * x'; %pinv(x);

%b = c' * xtxi * x' * y; % REDUCES to input vector true, regardless of filtering; b = true';
b = c' * px * y;

% i think this one below has V as the sqrt of the V in Friston
%sx = sqrt(diag(c' * xtxi * x' * V' * V * x * xtxi * c))   % design part -- std of x, sqrt(var(x))
sx = sqrt(diag(c' * px * V * px' * c));

% %n = b ./ sx;    % noncentrality
% mod 2/07
n = b ./ (wsd .* sx);

% GETTING THE NONCENTRALITY PARAMETER n
% noncentrality parameter n = snr * b/se
% snr = |h| / sigma, where |h| = scaling of impulse response to neural
% event.  see Zarahn 2001, "Reference Effect for Power Analysis"
% We can simplify the computation by always assuming |h| = 1 and varying
% sigma, in the simplest case setting snr = 1.
%
% t-values are directly related to snr and n.
% t = (|h| * n) / sigma
% in the simplest case, where snr = 1, t = n.
%
% n is contrast weight invariant.  contrasts can be normalized by sum of
% squared contrast weights.
%
% FROM n to POWER
% By the central limit theorem, n = t if snr = 1, and n is normally
% distributed with std 1.  Then we can integrate the normal distribution.
%
% but since the noncentrality param is essentially a t-value, as is the 20%
% level for the 80% power value, we have to do a small additional
% adjustment: convert t to Z scores based on the df.

% Degrees of freedom: Worsley and Friston (1995) way (effective df),
% df reduction due to autocorrelation.  This will be different if
% a prewhitening strategy is used.

R = eye(size(x,1)) - x * xtxi * x';
U = R * V * V';
df = (trace(U) .^ 2) ./ (trace(U * U));

%df = size(x,1) - size(x,2);  % standard way, but not correct with
%autocorrelated errors

%t80_ind = norminv(.2,n,1);
% mod 2/07
t80_ind = nctinv(.2,df,n);  % 20th% of noncentral t distribution
ztmp = spm_t2z(t80_ind,df);
ztmp = min(ztmp,t80_ind);    % use min, as conversion fails for high-power

% % tmp = norminv(tcdf(t80_ind,df));
% % tmp = min(tmp,t80_ind);             % use min, as conversion fails for high-power
% %
varargout{1} = ztmp;

% GROUP POWER
% group power is calculated by considering the contribution of the
% individual se to the group se, and assuming a fixed intersubject variance (set nominally to 1).  with a
% with group rrue intersubject standard deviation (gsd) of 1, then
% gse = (gsd + E(indiv_se)) ./ sqrt(2N)
% by the central limit theorem.
% we use sqrt(2N) because std dev of n rnd variables = (s1 + s2 ... sn) / sqrt(n)
% individual se (ise) is the standard deviation of the sampling distribution
% gse is the standard deviation
%
% take as ise the coefficient of variation (cv), sx / b, which is contrast
% scale independent.

if nargout > 1

    %gse = (gsd + (sx ./ b)) ./ sqrt(2 * N);

    %ng = 1 ./ gse;  % normalized b / gse.  this is contrast weight-scaling independent

    % group standard error.  sqrt of sum of within + between, over
    % sqrt(N-1) to get standard error from standard deviation
    gse = sqrt( (wsd.*sx).^2 + gsd.^2 ) ./ sqrt(N - 1);

    ng = true(1:end-1)' ./ gse;  %  b / gse.  this is contrast weight-scaling independent

    dfg = N - 1;
    t80_grp = nctinv(.2, dfg, ng);  % 20th% of noncentral t distribution
    ztmp = spm_t2z(t80_grp,dfg);
    ztmp = min(ztmp,t80_grp);    % use min, as conversion fails for high-power

% %     t80_grp = norminv(.2,ng,1);
% %
% %     tmp = norminv(tcdf(t80_grp,dfg));   % adjust to Z-score based on df
% %     tmp = min(tmp,t80_grp);             % use min, as conversion fails for high-power
    varargout{2} = ztmp;

    if nargout > 2

        OUT.x = x;
        OUT.gsd = gsd;
        OUT.N = N;
        OUT.true = true;
        OUT.c = c';
        OUT.y = y;
        OUT.b = b;
        OUT.sx = sx;
        OUT.n = n;
        OUT.df = df;
        OUT.ng = ng;

        OUT.t80_ind = t80_ind;
        OUT.t80_grp = t80_grp;
        OUT.expected_t = b ./ (wsd .* sx);

        varargout{3} = OUT;

    end

end

return


function [grad_X, grad_W, grad_b] = B_weighting(prev_layer, curr_layer, future_layers)

input = prev_layer{1}.a;
weight = curr_layer.W;
nWeight = length(weight);
D = size(input,1)/nWeight;

grad_X = 0; grad_W=0; grad_b=0;
future_grad = GetFutureGrad(future_layers, curr_layer);
if 0
    for i=1:length(future_layers)
        if curr_layer.update
            for j=1:size(input,2)
                curr_input = reshape(input(:,j), D, nWeight);
                grad_W = grad_W + curr_input' * future_layers{i}.grad(:,j);
            end
            if isfield(curr_layer, 'updateBias')== 0 || curr_layer.updateBias==1
                grad_b = grad_b + sum(future_layers{i}.grad,2);
            end
        end
        for j=1:size(input,2)
            grad_X = grad_X + future_layers{i}.grad(:,j)*weight';
        end
    end
else
    if curr_layer.update
        for j=1:size(input,2)
            curr_input = reshape(input(:,j), D, nWeight);
            grad_W = grad_W + curr_input' * future_grad(:,j);
        end
        if isfield(curr_layer, 'updateBias')== 0 || curr_layer.updateBias==1
            grad_b = grad_b + sum(future_grad,2);
        end
    end
    for j=1:size(input,2)
        grad_X = grad_X + future_grad(:,j)*weight';
    end
end
grad_X = reshape(grad_X, prod(size(grad_X)), 1);

end


function linplus_test152 ( )

%*****************************************************************************80
%
%% TEST152 tests R8BLT_INDICATOR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 6;
  ml = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST152\n' );
  fprintf ( 1, '  R8BLT_INDICATOR sets up a R8BLT indicator matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N     = %d\n', n );
  fprintf ( 1, '  Lower bandwidth ML = %d\n', ml );
%
%  Set the matrix.
%
  a = r8blt_indicator ( n, ml );

  r8blt_print ( n, ml, a, '  The indicator matrix:' );

  return
end


% Features computation for the Harmonic Model + Phase Distortion (HMPD)
%
% Octave compatible
%
% Please read the README.txt file for general information about HMPD before
% using it.
%
% Inputs
%  frames : [Nxstruct] N structures containing the sinusoidal parameters,
%           as provided by hmpd_analysis_harmonic.m or another sinusoidal
%           analysis.
%  fs     : [Hz] The sampling rate of the analyzed waveform
%  [opt]  : Additional options (see code below)
%           Even though this argument is optional, it is highly recommended to
%           adapt the f0min and f0max options if no f0s argument is provided.
%
% Outputs
%  f0s   : [s, Hz] [Nx2] A time/data column vector, as above, containing the
%          analysis instants and the f0 curve.
%  AE    : [NxD] A matrix containing the amplitude envelope.
%          D is either opt.dftlen/2+1 (from hmpd_features_compute.m), or
%          opt.amp_order+1, depending if compression is disabled or enabled.
%  PDM   : [NxD] A matrix containing the Phase Distortion Mean.
%          D is either opt.dftlen/2+1 or opt.pdm_order+1, depending if
%          compression is disabled or enabled.
%  PDD   : [NxD] A matrix containing the Phase Distortion Deviation.
%          D is either opt.dftlen/2+1 or opt.pdd_order+1, depending if
%          compression is disabled or enabled.
%  opt   : The options used for the analysis.
%
% Copyright (c) 2013 University of Crete - Computer Science Department (UOC-CSD)
%
% License
%  This file is under the LGPL license,  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. This file 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.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function [f0s, AE, PDM, PDD, opt] = hmpd_analysis_features(frames, fs, opt)
%  keyboard
    if opt.debug>0; disp('HMPD Vocoder: Features computation ...'); end

    irregf0s = [[frames.t]', [frames.f0]']; % Retrieve f0 from the frames

    if opt.debug>0; disp('    Estimate amplitude envelope ...'); end
    [AE, frames] = hmpd_amplitude_envelope_estimate(frames, fs, opt);

    if opt.debug>0; disp('    Estimate phase envelope ...'); end
    rpspdopt = opt;
    rpspdopt.pd_method = 1;
    rpspdopt.pd_vtf_rm =false;% Already done in hmpd_amplitude_envelope_estimate
    rpspdopt.harm2freq = true;
    rpspdopt.usemex = opt.usemex;
    PE = phase_rpspd(frames, fs, rpspdopt);

    if opt.debug>0; disp('    Compute phase statistics ...'); end

    % Phase's mean
    PDM = hmpd_phase_mean(PE, opt.pdm_nbper*opt.sin_nbat); % Below 3, doubles the voice, why?

    % Phase's standard-deviation
    % Remove first the trend from PE, otherwise the std measure is overestimated
    PEtrend = hmpd_phase_smooth(PE, opt.pdd_nbper*opt.sin_nbat); % Compute the trend
    PE = PE - PEtrend;

    PDD = hmpd_phase_deviation(PE, opt.pdd_nbper*opt.sin_nbat);

    if opt.debug>0; disp('    Compress features ...'); end
    [AE, PDM, PDD] = hmpd_features_compress(irregf0s, AE, PDM, PDD, fs, opt);

    if opt.debug>0; disp('    Resample features with uniform sampling ...'); end
    uT = (0:opt.regularstepsize:irregf0s(end,1))'; % New uniform time instants

    % First, resample the f0 curve using uniform step size
    uf0s = exp(interp1(irregf0s(:,1), log(irregf0s(:,2)), uT, 'linear', NaN));

    % Drop the time instants outside of the features
    idx = find(~isnan(uf0s));
    uT = uT(idx);
    f0s = [uT, uf0s(idx)];

    % Then, resample amplitude, PD's mean and PD's deviation
    AE = irregsampling2uniformsampling(irregf0s(:,1), AE, uT, [], [], 'linear', NaN, opt.usemex);
    PDM = irregsampling2uniformsampling(irregf0s(:,1), PDM, uT, @unwrap, @wrap, 'linear', 0, opt.usemex);
    PDD = irregsampling2uniformsampling(irregf0s(:,1), PDD, uT, [], [], 'linear', 0, opt.usemex);

return


%MDL_JACO Create model of Kinova Jaco manipulator
%
% MDL_JACO is a script that creates the workspace variable jaco which
% describes the kinematic characteristics of a Kinova Jaco manipulator
% using standard DH conventions.
%
% Also define the workspace vectors:
%   qz         zero joint angle configuration
%   qr         vertical 'READY' configuration
%
% Reference::
% - "DH Parameters of Jaco" Version 1.0.8, July 25, 2013.
%
% Notes::
% - SI units of metres are used.
% - 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 SerialLink, mdl_mico, mdl_puma560.

% MODEL: Kinova, Jaco, 6DOF, standard_DH

% Copyright (C) 1993-2015, 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_jaco()

    deg = pi/180;

    % robot length values (metres)
    D1 = 0.2755;
    D2 = 0.4100;
    D3 = 0.2073;
    D4 = 0.0743;
    D5 = 0.0743;
    D6 = 0.1687;
    e2 = 0.0098;

    % alternate parameters
    aa = 30*deg;
    ca = cos(aa);
    sa = sin(aa);
    c2a = cos(2*aa);
    s2a = sin(2*aa);
    d4b = D3 + sa/s2a*D4;
    d5b = sa/s2a*D4 + sa/s2a*D5;
    d6b = sa/s2a*D5 + D6;

    % and build a serial link manipulator

    % offsets from the table on page 4, "Mico" angles are the passed joint
    % angles.  "DH Algo" are the result after adding the joint angle offset.

    robot = SerialLink([
        Revolute('alpha', pi/2,  'a', 0,  'd', D1,   'flip')
        Revolute('alpha', pi,    'a', D2, 'd', 0,    'offset', -pi/2)
        Revolute('alpha', pi/2,  'a', 0,  'd', -e2,  'offset', pi/2)
        Revolute('alpha', 2*aa,  'a', 0,  'd', -d4b)
        Revolute('alpha', 2*aa,  'a', 0,  'd', -d5b, 'offset', -pi)
        Revolute('alpha', pi,    'a', 0,  'd', -d6b, 'offset', 100*deg)
        ], ...
        'name', 'Jaco', 'manufacturer', 'Kinova');

    % place the variables into the global workspace
    if nargin == 1
        r = robot;
    elseif nargin == 0
        assignin('base', 'jaco', robot);
        assignin('base', 'qz', [0 0 0 0 0 0]); % zero angles
        assignin('base', 'qr', [270 180 180 0 0 0]*deg); % vertical pose as per Fig 2
    end
end


%  ARROWH   Draws a solid 2D arrow head in current plot.
%
%	 ARROWH(X,Y,COLOR,SIZE,LOCATION) draws a  solid arrow  head into
%	 the current plot to indicate a direction.  X and Y must contain
%	 a pair of x and y coordinates ([x1 x2],[y1 y2]) of two points:
%
%	 The first  point is only used to tell  (in conjunction with the
%	 second one)  the direction  and orientation of  the arrow -- it
%	 will point from the first towards the second.
%
%	 The head of the arrow  will be located in the second point.  An
%	 example of use is	plot([0 2],[0 4]); ARROWH([0 1],[0 2],'b')
%
%	 You may also give  two vectors of same length > 2.  The routine
%	 will then choose two consecutive points from "about" the middle
%	 of each vectors.  Useful if you  don't want to worry  each time
%	 about  where to  put the arrows on  a trajectory.  If x1 and x2
%	 are the vectors x1(t) and x2(t), simply put   ARROWH(x1,x2,'r')
%	 to have the right  direction indicated in your x2 = f(x1) phase
%	 plane.
%
%            (x2,y2)
%            --o
%            \ |
%	            \|
%
%
%		  o
%	  (x1,y1)
%
%	 Please note  that the following  optional arguments  need -- if
%	 you want  to use them -- to  be given in that exact order.  You
%	 may pass on empty vectors "[]" to skip arguments you don't want
%	 to set (if you want to access "later" arguments...).
%
%	 The COLOR argument is quite the same as for plots,  i.e. either
%  a string like  'r' or an RGB value vector like  [1 0 0]. If you
%  only want the outlines of the head  (in other words a non-solid
%  arrow head), prefix the color string by 'e' or the color vector
%  by 0, e.g. to get only a red outline use 'er' or [0 1 0 0].
%
%	 The SIZE argument allows you to tune the size of the arrows. If
%	 SIZE is a scalar, it scales the arrow proportionally.  SIZE can
%	 also be  a two element vector,  where the first element  is the
%	 overall  scale (in percent),  the second one controls the width
%	 of the arrow head (again, in percent).
%
%	 The LOCAITON argument can be used to tweak the position  of the
%  arrow head.  If a time series of x and y coordinates are given,
%  you can use this argument  to place the arrow head for instance
%  at 20% along the line.  It can be a vector, if you want to have
%  more than one arrow head drawn.
%
%	 Both SIZE and LOCATION arguments must also be given in percent,
%	 where 100 means standard size, 50 means half size, respectively
%	 100 means end of the vector, 0 beginning of it. Note that those
%	 "locations" correspond to the cardinal position "inside" the
%	 vector, in other words the "index-wise" position.
%
%	 This little tool is mainely intended  to be used for indicating
%	 "directions" on trajectories -- just give two consecutive times
%	 and the corresponding values of a flux and the proper direction
%	 of the trajectory will be shown on the plot.  You may also pass
%	 on two solution vectors, as described above.
%
%	 Note, that the arrow  heads only look good in the original axis
%	 settings (as in when the routine was actually started).  If you
%	 zoom in afterwards, the triangle will get distorted.
%
%  HANDLES = ARROWH(...)  will give you a vector with  the handles
%  to the patches created by this function  (if you want to modify
%  them later on, for instance).
%
%	 Examples of use:
% 	 x1 = [0:.2:2]; x2 = [0:.2:2]; plot(x1,x2); hold on;
% 	 arrowh(x1,x2,'r',[],20);            % passing entire vectors
% 	 arrowh([0 1],[0 1],'eb',[300,75]);  % passing 2 points
% 	 arrowh([0 1],[0 1],'eb',[300,75],25); % head closer to (x1,y1)

%	 Author:     Florian Knorn
%	 Email:      florian@knorn.org
%	 Version:    1.14
%	 Filedate:   Jun 18th, 2008
%
%	 History:    1.14 - LOCATION now also works with lines
%              1.13 - Allow for non-solid arrow heads
%              1.12 - Return handle(s) of created patches
%              1.11 - Possibility to change width
%	             1.10 - Buxfix
%	             1.09 - Possibility to chose *several* locations
%	             1.08 - Possibility to chose location
%	             1.07 - Choice of color
%	             1.06 - Bug fixes
%	             1.00 - Release
%
%	 ToDos:      - Keep proportions when zooming or resizing; has to
%	               be done with callback functions, I guess.
%
%	 Bugs:       None discovered yet, those discovered were fixed
%
%	 Thanks:     Thanks  also  to Oskar Vivero  for using  my humble
%	             little program in his great MIMO-Toolbox.
%
%	 If you have  suggestions for  this program,  if it doesn't work
%	 for your "situation" or if you change something in it -- please
%	 send me an email!  This is my very  first "public" program  and
%	 I'd  like to  improve it where  I can -- your  help is  kindely
%	 appreciated! Thank you!

function handle = arrowh(x,y,clr,ArSize,Where)

%-- errors
if nargin < 2
	error('Please give enough coordinates !');
end
if (length(x) < 2) || (length(y) < 2),
	error('X and Y vectors must each have "length" >= 2 !');
end
if (x(1) == x(2)) && (y(1) == y(2)),
	error('Points superimposed - cannot determine direction !');
end
if nargin <= 2
	clr = 'b';
end
if nargin <= 3
	ArSize = [100,100];
end

handle = [];

%-- check if variables left empty, deal width ArSize and Color
if isempty(clr)
	clr = 'b'; nonsolid = false;
elseif ischar(clr)
	if strncmp('e',clr,1) % for non-solid arrow heads
		nonsolid = true; clr = clr(2);
	else
		nonsolid = false;
	end
elseif isvector(clr)
	if length(clr) == 4 && clr(1) == 0  % for non-solid arrow heads
		nonsolid = true;
		clr = clr(2:end);
	else
		nonsolid = false;
	end
else
	error('COLOR argument of wrong type (must be either char or vector)');
end

if nargin <= 4
	if (length(x) == length(y)) && (length(x) == 2)
		Where = 100;
	else
		Where = 50;
	end
end

if isempty(ArSize)
	ArSize = [100,100];
end
if length(ArSize) == 2
	ArWidth = 0.75*ArSize(2)/100; % .75 to make arrows it a bit slimmer
else
	ArWidth = 0.75;
end
ArSize = ArSize(1);

%-- determine and remember the hold status, toggle if necessary
if ishold,
	WasHold = 1;
else
	WasHold = 0;
	hold on;
end

%-- start for-loop in case several arrows are wanted
for Loop = 1:length(Where),

	%-- if vectors "longer" then 2 are given we're dealing with time series
	if (length(x) == length(y)) && (length(x) > 2),
		j = floor(length(x)*Where(Loop)/100); %-- determine that location
		if j >= length(x), j = length(x) - 1; end
		if j == 0, j = 1; end
		x1 = x(j); x2 = x(j+1); y1 = y(j); y2 = y(j+1);

	else %-- just two points given - take those
		x1 = x(1); x2 = (1-Where/100)*x(1)+Where/100*x(2);
		y1 = y(1); y2 = (1-Where/100)*y(1)+Where/100*y(2);
	end

	%-- get axe ranges and their norm
	OriginalAxis = axis;
	Xextend = abs(OriginalAxis(2)-OriginalAxis(1));
	Yextend = abs(OriginalAxis(4)-OriginalAxis(3));

	%-- determine angle for the rotation of the triangle
	if x2 == x1, %-- line vertical, no need to calculate slope
		if y2 > y1,
			p = pi/2;
		else
			p= -pi/2;
		end
	else %-- line not vertical, go ahead and calculate slope
		%-- using normed differences (looks better like that)
		m = ( (y2 - y1)/Yextend ) / ( (x2 - x1)/Xextend );
		if x2 > x1, %-- now calculate the resulting angle
			p = atan(m);
		else
			p = atan(m) + pi;
		end
	end

	%-- the arrow is made of a transformed "template triangle".
	%-- it will be created, rotated, moved, resized and shifted.

	%-- the template triangle (it points "east", centered in (0,0)):
	xt = [1	-sin(pi/6)	-sin(pi/6)];
	yt = ArWidth*[0	 cos(pi/6)	-cos(pi/6)];

	%-- rotate it by the angle determined above:
	xd = []; yd = [];
	for i=1:3
		xd(i) = cos(p)*xt(i) - sin(p)*yt(i);
		yd(i) = sin(p)*xt(i) + cos(p)*yt(i);
	end

	%-- move the triangle so that its "head" lays in (0,0):
	xd = xd - cos(p);
	yd = yd - sin(p);

	%-- stretch/deform the triangle to look good on the current axes:
	xd = xd*Xextend*ArSize/10000;
	yd = yd*Yextend*ArSize/10000;

	%-- move the triangle to the location where it's needed
	xd = xd + x2;
	yd = yd + y2;

	%-- draw the actual triangle
	handle(Loop) = patch(xd,yd,clr,'EdgeColor',clr);
	if nonsolid, set(handle(Loop),'facecolor','none'); end
end % Loops

%-- restore original axe ranges and hold status
axis(OriginalAxis);
if ~WasHold,
	hold off
end

%-- work done. good bye.


function [ var_node, var_num ] = ns_t6_var_count ( element_num, ...
  element_node, node_num )

%*****************************************************************************80
%
%% NS_T6_VAR_COUNT counts the Navier Stokes variables on a T6 grid.
%
%  Discussion:
%
%    We are given a mesh of T6 elements, and asked to count, in advance,
%    the number of Navier-Stokes variables associated with the grid.
%    In particular, every node has two velocity variables associated with
%    it, but only a node that is a vertex of the element will also have
%    an associated pressure variable.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM);
%    ELEMENT_NODE(I,J) is the global index of local node I in element J.
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Output, integer VAR_NODE(NODE_NUM+1), used to find the variables
%    associated with a given node, which are in VAR in locations
%    VAR_NODE(NODE) to VAR_NODE(NODE+1)-1.  Note that the last entry of
%    this array points to the location just after the last location in VAR.
%
%    Output, integer VAR_NUM, the number of variables.
%
  element_order = 6;
%
%  Our job is easy once we determine which nodes are vertices.
%  So to begin with, let VAR_NODE count the number of variables
%  associated with each node.
%
  var_node(1:node_num) = 2;

  for element = 1 : element_num
    for order = 1 : 3
      node = element_node(order,element);
      var_node(node) = 3;
    end
  end
%
%  Count them.
%
  var_num = sum ( var_node(1:node_num) );
%
%  Make pointers.
%
  total = 1;

  for node = 1 : node_num
    num = var_node(node);
    var_node(node) = total;
    total = total + num;
  end
  var_node(node_num+1) = total;

  return
end


function [B,elapse] = ITQ_compress(A, model)
%   This is a wrapper function of ITQ testing.
%
%	Usage:
%	[B,elapse] = ITQ_compress(A, model)
%
%	      A: Rows of vectors of data points. Each row is sample point
%     model: The model generated by ITQ_learn.
%
%	      B: The binary code of the input data A. Each row is sample point
%    elapse: The coding time (testing time).
%
%
%
%   version 2.0 --Nov/2016
%   version 1.0 --Jan/2013
%
%   Written by  Yue Lin (linyue29@gmail.com)
%               Deng Cai (dengcai AT gmail DOT com)
%

tmp_T = tic;

Ym = A * model.pc;
Ym = Ym * model.R;
B = (Ym > 0);

elapse = toc(tmp_T);
end


%compute the ycoordinate of the center in the tails coordinate system

function [data,units]=compute_ytailcentral_mm(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
ytailcentral_mm=cell(1,numlarvae);

for i=1:numlarvae
    larva=larvae(i);
    ytailcentral_mm{1,i}=trx(larva).ycentral_mm-trx(larva).ytail_mm;
end

units=parseunits('mm');
data=ytailcentral_mm;


function result = normalizekm(km)
% Copyright 2012 Nino Shervashidze, Karsten Borgwardt
% normalizes kernelmatrix km
% such that diag(result) = 1, i.e. K(x,y) / sqrt(K(x,x) * K(y,y))
% @author Karsten Borgwardt
% @date June 3rd 2005
% all rights reserved

nv = sqrt(diag(km));
nm =  nv * nv';
knm = nm .^ -1;
for i = 1:size(knm,1)
for j = 1:size(knm,2)
if (knm(i,j) == Inf)
  knm(i,j) = 0;
end
end
end
result = km .* knm;


function c = convv(a,b);
%function c = convv(a,b);
%
%Convolves 3-D matrices a and b

% S. de Waele, March 2003.

sa = kingsize(a);
sb = kingsize(b);
s = sb;
s(3) = sa(3)+sb(3)-1;
b_z = zeros(s);

b_z(:,:,1:sb(3)) = b;
c = armafilterv(b_z,1,a);


function [V,D,permutation,failure] = dmpermblockeig(X,switchtosparse)

[permutation,aux1,aux2,blocks] = dmperm(X+speye(length(X)));
Xpermuted = X(permutation,permutation);

V = [];
D = [];
V = zeros(size(X,1),1);
top = 1;
left = 1;
anycholfail = 0;
failure = 0;

for i = 1:length(blocks)-1
    Xi = Xpermuted(blocks(i):blocks(i+1)-1,blocks(i):blocks(i+1)-1);
    [R,fail] = chol(Xi);
    anycholfail = anycholfail | fail;
    if fail && nnz(Xi)>0
        if length(Xi) >= switchtosparse
            [vi,di,eigfail] = eigs(Xi,5,'SA');
            if eigfail || isempty(di)
                res = 0;
                for j = 1:size(vi,2)
                    res(j) = norm(Xi*vi(:,j)-vi(:,j)*di(j,j));
                end
                % We only trust these
                notfailed = abs(res) <= 1e-12;
                vi = vi(:,notfailed);
                di = di(notfailed,notfailed);
                if length(vi) == 0
                    [vi,di,eigfail] = eigs(sparse(Xi),25,'SA');
                    if eigfail
                        res = 0;
                        for j = 1:size(vi,2)
                            res(j) = norm(Xi*vi(:,j)-vi(:,j)*di(j,j));
                        end
                        % We only trust these
                        notfailed = abs(res) <= 1e-12;
                        vi = vi(:,notfailed);
                        di = di(notfailed,notfailed);
                    end
                end
            end
        else
            [vi,di] = eig(full(Xi));
        end
        for j = 1:length(di)
            if di(j,j)<=0
                V(top:top+length(Xi)-1,left)=vi(:,j);
                left = left + 1;
                D = blkdiag(D,di(j,j));
            end
        end
    end
    top = top + length(Xi);
end

if (anycholfail && isempty(V)) || (anycholfail && all(diag(D)>0))
    % OK, we have a problem. The Cholesky factorization failed for some of
    % the matrices, but yet no eigenvalue decomposition revealed a negative
    % eigenvalue (due to convergence issues in the sparse eigs)
    failure = 1;
end


function [oz] = l2oz(l)
% Convert volume from liters to US liquid ounces.
% Chad Greene 2012
oz = l*33.814022701;


function out = lsb_dec( wavin, pass )
%LSB_DEC LSB Coding technique for WAV files using one bit to hide
%
%   INPUT VARIABLES
%       wavin : Path of stego signal (wav input)
%       pass  : Password for decryption
%
%   OUTPUT VARIABLES
%       out   : Retrieved message
%
%   Kadir Tekeli (kadir.tekeli@outlook.com)

if (nargin<2)
    pass = 'password123';
end

%Header = 1:40, Length = 41:43, Data = 44:end
fid = fopen(wavin,'r');
header = fread(fid,40,'uint8=>char');
dsize  = fread(fid,1,'uint32');
stego  = fread(fid,inf,'uint16');
fclose(fid);
%Control variable is read (1:8)
control = bitget(stego(1:8),1)';

if b2d(control) == mod(sum(double(pass)),256)
    %Length of message is read (9:48)
    m = bitget(stego(9:48),1);
    len = b2d(m')*8;
    %Hidden message is read and decrypted (49:len)
    dat = xor(bitget(stego(49:48+len),1),prng(pass,len));
    bin = reshape(dat,len/8,8);
    out = char(b2d(bin))';
else
    warning('Password is wrong or message is corrupted!');
    out = [];
end
end

function d = b2d(b)
%B2D Minimal implentation of bi2de function
  if isempty(b)
    d = [];
  else
    d = b * (2 .^ (0:length(b(1,:)) - 1)');
  end
end

function out = prng( key, L )
%PRNG Pseudorandom number generator for encryption/decryption
pass = sum(double(key).*(1:length(key)));
rand('seed', pass);
out = (rand(L, 1)>0.5);
end


function mpc = ex_case3b
%EX_CASE3B  Three bus example system for stochastic unit commitment.
%   Same as EX_CASE3A with the following changes:
%       - Non-zero PMIN values for generators
%       - Linear (vs quadratic) generator costs
%       - Includes some STARTUP and SHUTDOWN costs
%   Please see CASEFORMAT for details on the case file format.

%   MOST
%   Copyright (c) 2015-2016, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MOST.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/most for more info.

%% MATPOWER Case Format : Version 2
mpc.version = '2';

%%-----  Power Flow Data  -----%%
%% system MVA base
mpc.baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [
	1	3	0	0	0	0	1	1	0	135	1	1.05	0.95;
	2	2	0	0	0	0	1	1	0	135	1	1.05	0.95;
	3	2	0	0	0	0	1	1	0	135	1	1.05	0.95;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin	Pc1	Pc2	Qc1min	Qc1max	Qc2min	Qc2max	ramp_agc	ramp_10	ramp_30	ramp_q	apf
mpc.gen = [
	1	125	0	25	-25	1	100	1	200	60	0	0	0	0	0	0	0	250	250	0	0;
	1	125	0	25	-25	1	100	1	200	65	0	0	0	0	0	0	0	250	250	0	0;
	2	200	0	50	-50	1	100	1	500	60	0	0	0	0	0	0	0	600	600	0	0;
	3	-450	0	0	0	1	100	1	0	-450	0	0	0	0	0	0	0	500	500	0	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [
	1	2	0.005	0.01	0	300	300	300	0	0	1	-360	360;
	1	3	0.005	0.01	0	240	240	240	0	0	1	-360	360;
	2	3	0.005	0.01	0	300	300	300	0	0	1	-360	360;
];

%%-----  OPF Data  -----%%
%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	2	0	0	2	25	0;
	2	200	200	2	30	0;
	2	3000	600	2	40	0;
	2	0	0	2	1000	0;
];

%%-----  Reserve Data  -----%%
%% reserve zones, element i, j is 1 if gen j is in zone i, 0 otherwise
mpc.reserves.zones = [
	1	1	1	0;
];

%% reserve requirements for each zone in MW
mpc.reserves.req   = 150;

%% reserve costs in $/MW for each gen that belongs to at least 1 zone
%% (same order as gens, but skipping any gen that does not belong to any zone)
% mpc.reserves.cost  = [	5;	5;	21;	];
% mpc.reserves.cost  = [	5;	5;	16.25;	];
% mpc.reserves.cost  = [	0;	0;	11.25;	];
mpc.reserves.cost  = [	1;	3;	5;	];

%% OPTIONAL max reserve quantities for each gen that belongs to at least 1 zone
%% (same order as gens, but skipping any gen that does not belong to any zone)
mpc.reserves.qty   = [	100;	100;	200;	];


function totalEnergy = getEnergyDeposited(planC,structNum,doseNum)
%function getEnergyDeposited(planC,structNum,doseNum)
%
%This function computes the total Energy deposited within a structure.
%
%APA, 04/21/2010
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

global stateS
stateS.optS = opts4Exe([getCERRPath,'CERROptions.json']);
stateS.MLVersion = getMLVersion;

indexS = planC{end};

scanNum = getStructureAssociatedScan(structNum,planC);

%Get Uniformized dose matrix within the structure structNum
dose3M = getUniformDose(doseNum, scanNum, structNum, planC);

numVoxels = length(find(dose3M > 0));

%Get Uniformized scan matrix for scanNum
scan3M = getUniformizedCTScan(1, scanNum, planC);

[xV,yV,zV] = getUniformScanXYZVals(planC{indexS.scan}(scanNum));

dx = abs(xV(1)-xV(2));
dy = abs(yV(1)-yV(2));
dz = abs(zV(1)-zV(2));

voxelVolume = dx * dy * dz;

totalEnergy = sum(dose3M(:).*scan3M(:))*voxelVolume*numVoxels;


function [nodes, edges] = readGraph(fileName)
%READGRAPH Read a graph from a text file.
%
%   [NODES EDGES] = readGraph(FILENAME)
%
%   Example
%     % create a basic graph, save it to a file, and read it again
%     nodes = [10 10;20 10;10 20;20 20;27 15];
%     edges = [1 2;1 3;2 4;2 5;3 4;4 5];
%     writeGraph(nodes, edges, 'simpleGraph.txt');
%     [n2 e2] = readGraph('simpleGraph.txt');
%     figure; drawGraph(n2, e2); axis equal; axis([0 40 0 30]);
%
%   See also
%     writeGraph
%

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2014-01-21, using Matlab 7.9.0.529 (R2009b)
% Copyright 2014-2022 INRA - Cepia Software Platform

%% Open file and read header

% open file for reading in text mode
f = fopen(fileName, 'rt');
if f == -1
    error(['could not open file for reading: ' fileName]);
end

% check header
line = fgetl(f);
if ~ischar(line)
    error(['can not read graph from empty file: ' fileName]);
end
if ~strcmpi(strtrim(line(2:end)), 'graph')
    error(['Wrong header line in file:' fileName]);
end

%% read node section

% check sub-header
line = fgetl(f);
if ~strcmpi(strtrim(line(2:end)), 'nodes')
    error(['Could not interpret node section in file:' fileName]);
end

% read the number of nodes
line = fgetl(f);
[nNodesStr, line] = strtok(line);
nNodes = str2double(nNodesStr);

% read number of dimension, assumes 2 by default if not specified
nDims = 2;
if ~isempty(line)
    nDims = str2double(strtok(line));
end

% read node coordinates
[nodes, nRead] = fscanf(f, '%g', [nDims nNodes]);
assert(nRead == nNodes * nDims, ...
    'Could not read all node info in file %s', fileName);
nodes = nodes';

% terminate the reading of current line
fgetl(f);

%% read edge section

% check sub-header
line = fgetl(f);
if ~strcmpi(strtrim(line(2:end)), 'edges')
    error(['Could not interpret edge section in file:' fileName]);
end

% read the number of nodes
line = fgetl(f);
nEdgesStr = strtok(line);
nEdges = str2double(nEdgesStr);

% read node indices of each edge
[edges, nRead] = fscanf(f, '%d %d\n', [2 nEdges]);
assert(nRead == nEdges * 2, ...
    'Could not read all edge info in file %s', fileName);
edges = edges';

%% Clean up

fclose(f);


function mtrFigureSmoothness(kSmooth,saveDir)

%Definitions
%numSamples = 2000;
bSaveImages = 1;

% Create temp directory currently if none provided
% if ieNotDefined(tempDir)
%     tempDir = 'temp';
% end
% mkdir(tempDir);
% genPlotsSphDist(kSmooth,numSamples,tempDir);
genPlotsSphFunc(kSmooth);
set(gcf,'Position',[504   751   354   195]);
if bSaveImages
    figFilename = fullfile(saveDir,['smoothPdf.png']);
    set(gcf,'PaperPositionMode','auto');
    print('-dpng', figFilename);
end
return;

function genPlotsSphFunc(kSmooth)
 % Going to color a sphere according to the desired distribution
figure;
%cmap = [autumn(255); [.25 .25 .25]];
colormap(gray);
[px py pz] = sphere(80);
testVec = [px(:) py(:) pz(:)]';
watValues = reshape(watsonPdf([0,0,1]',testVec,kSmooth),size(pz));
% Get hemisphere
pz(pz<0) = 0;
surf(px,py,pz,reshape(watValues,size(px)),'EdgeAlpha',0,'FaceAlpha',1);
colorbar;
caxis([0,max(watValues(:))]);
axis equal
axis vis3d
grid on;

return;

function genPlotsSphDist(kSmooth,numSamples,tempDir)

% Create script for sampling vectors according to desired distribution
tempScriptFile = fullfile(tempDir,'tempRun.sh');
fid = fopen(tempScriptFile,'wt');
fprintf(fid,'#!/bin/bash\n');
fprintf(fid,'sphsample -pdf watson -k %g -ns %g -outputfile tempDist.Bdouble\n',kSmooth,numSamples);
fclose(fid);
input(['Run ' tempScriptFile ' then press [return] when finished to continue.']);

% Cleanup and load the sample data
delete(tempScriptFile);
figure;
cmap = [autumn(255); [.25 .25 .25]];
sampleFile = fullfile(tempDir,'tempDist.Bdouble');
displaySphDist(sampleFile,numSamples);
%set(gca,'Postion',[0.13   0.11   0.775   0.815]);
title(['\DeltaS = ' num2str(round(180 / (pi*sqrt(kSmooth)))) ' \circ']);
delete(sampleFile);
return;

function displaySphDist(sampleFile,numSamples)

fid = fopen(sampleFile,'rb','b');
d = fread(fid,'double'); fclose(fid);
vecs = reshape(d,3,numSamples);
sub_vecs = vecs(:,vecs(3,:)>0);
plot3(sub_vecs(1,:), sub_vecs(2,:), sub_vecs(3,:), '.')
hold on
[px py pz] = sphere;
surf(px,py,pz,repmat(256,size(pz)),'EdgeAlpha',1,'FaceAlpha',0);
%alpha(0.3)
axis equal
axis vis3d
grid off;
% xlabel('x');
% ylabel('y');
% zlabel('z');

return;


function [M_, S_]=Prior2Posterior(M,Q,M_Q,S,G,S_G)

M_=M+S*Q'*inv(Q*S*Q')*(M_Q-Q*M);
S_=S+(S*G')*( inv(G*S*G')*S_G*inv(G*S*G') -inv(G*S*G') )*(G*S);


load hershey
palabra='D';
sz=size(palabra);
figure
%hold on
xprev=0;
%yprev=0;
% accede a cada letra B
for i= 1:sz(2)
B = hershey{palabra(i)};

%Se muestran por pantalla las coordenadas y se marca con NaN cada uno de
%los segmentos de la letra. Son los trazos que pueden dibujarse sin
%levantar el lapiz de la superficie
B.stroke;
%X=[(B.stroke(1,:)+xprev+0.2) B.stroke(2,:)].*3; %Esfera
%x= [2*X(1)/(1+X(1)^2+X(2)^2), 2*X(2)/(1+X(1)^2+X(2)^2), (-1+X(1)^2+X(2)^2)/(2+X(1)^2+X(2)^2)];

%obten coordenadas "unitarias" x, y
x=B.stroke(1,:)+xprev+0.2;
y=B.stroke(2,:);

%plotea mostrando las coordenadas de los puntos en coordenadas unitarias
%%figure
hold on
%plot(x,y,'*')
xprev=max(x);

end
% Con las lineas siguientes obtenemos una trayectoria que permite levantar
% el lapiz cuando se finaliza cada trazo
path = [ 0.25*B.stroke; zeros(1,size(B.stroke,2))];
k = find(isnan(path(1,:)));
path(:,k) = path(:,k-1);
path(3,k) = 0.2;

% % plotea el camino
plot3(path(1,:),path(2,:), path(3,:),'*')


function tests = PGraphTest
  tests = functiontests(localfunctions);
end
function PGraph2_test(testCase)

    stream = RandStream.getGlobalStream;
    stream.reset()

    g = PGraph(2);

    % add first component, nodes 1-5
    g.add_node( rand(2,1));
    g.add_node( rand(2,1));
    g.add_node( rand(2,1));
    g.add_node( rand(2,1));
    g.add_node( rand(2,1));

    g.add_edge(1,2);
    g.add_edge(1,3);
    g.add_edge(1,4);
    g.add_edge(2,3);
    g.add_edge(2,4);
    g.add_edge(4,5);

    verifyEqual(testCase, g.n, 5);
    verifyEqual(testCase, g.ne, 6);
    verifyEqual(testCase, g.nc, 1);
    verifyEqual(testCase, g.neighbours(2), [3 4 1]);
    z = g.coord(1);
    %verifyEqual(testCase,  g.distance(1,2), 0.6878, 'absTol',1e-4)
%     verifyEqual(testCase,  g.distances([0 0]), ...
%         [0.6137    0.6398    0.9222    1.2183    1.3593], 'absTol',1e-4)

    D = g.degree();
    I = g.incidence();
    A = g.adjacency();
    L = g.laplacian();

    verifyEqual(testCase, D, diag([3 3 2 3 1]));
    verifyEqual(testCase, sum(I), [2 2 2 2 2 2]);
    verifyEqual(testCase, sum(I'), [3 3 2 3 1]);
    verifyEqual(testCase, max(max(A-A')), 0);

    verifyEqual(testCase,  g.edgedir(1,3), 1);
    verifyEqual(testCase,  g.edgedir(3,1), -1);
    verifyEqual(testCase,  g.edgedir(2,5), 0);
    verifyEqual(testCase, g.neighbours_d(2), [-1 3 4]);

    s = g.char();

    % test node data
    g.setvdata(1, [1 2 3]);
    d = g.vdata(1);
    verifyEqual(testCase, d, [1 2 3]);

    % test edge data
    g.setedata(1, [4 5 6 7]);
    d = g.edata(1);
    verifyEqual(testCase, d, [4 5 6 7]);

    % add second component, nodes 6-8

    g.add_node( rand(2,1));
    g.add_node( rand(2,1), 6, 1);
    g.add_node( rand(2,1), 7);
    g.add_edge(6,7);
    g.add_edge(6,8);

    verifyEqual(testCase, g.nc, 2);

    clf
    g.plot('labels')
    g.plot('edgelabels')
    g.plot('componentcolor')

    [path,cost]=g.Astar(3,5);
    g.highlight_path(path);
    %verifyEqual(testCase, path, [3 2 4 5]);
    %verifyEqual(testCase,  cost, 2.1536, 'absTol',1e-4)

    %verifyEqual(testCase, g.closest([0 0]), 4);

    g.setcost(1, 99);
    verifyEqual(testCase,  g.cost(1), 99);

    g.Astar(3, 5);
    %verifyEqual(testCase, path, [3 2 4 5]);
end

function PGraph3_test(testCase)

    stream = RandStream.getGlobalStream;
    stream.reset()

    g = PGraph(3);

    g.add_node( rand(3,1));
    g.add_node( rand(3,1));
    g.add_node( rand(3,1));
    g.add_node( rand(3,1));
    g.add_node( rand(3,1));

    g.add_edge(1,2);
    g.add_edge(1,3);
    g.add_edge(1,4);
    g.add_edge(2,3);
    g.add_edge(2,4);
    g.add_edge(4,5);

    clf
    g.plot('labels')
    g.plot('edgelabels')
end


function grad = B_SpatialCovMask(future_layers, prev_layers, curr_layer)
mask = prev_layers{1}.a;
data = prev_layers{2}.a;
[D,T,N] = size(mask);
[D2,T,N] = size(data);
nCh = D2/D;
data = reshape(data, D, nCh, T, N);
data = permute(data, [2 3 1 4]);
% data = abs(data);

mask_f = sum(mask,2);     % sum of mask over time

spatCov = curr_layer.a;
speechCov = reshape(spatCov(1:D*nCh^2,:,:,:), nCh^2,1,D,N);
noiseCov = reshape(spatCov(D*nCh^2+1:end,:,:,:), nCh^2,1,D,N);

future_grad = GetFutureGrad(future_layers, curr_layer);
future_grad_speech = reshape(future_grad(1:D*nCh^2,:,:), nCh^2,1,D,N);
future_grad_noise = reshape(future_grad(D*nCh^2+1:end,:,:), nCh^2,1,D,N);

% from speech covariance
if 0    % slow implementation
    data = gather(data);
    data_cell = num2cell(data, [1]);
    tic; xx = cellfun(@(x) (reshape(x*x',nCh^2,1)), data_cell, 'UniformOutput', 0);toc;
    xx2 = cell2mat(xx);
else    % really fast implementation
    xx2 = reshape(outProdND(data), nCh^2, T,D);
end

gradFromSpeech = bsxfun(@minus, xx2, speechCov);
gradFromSpeech = bsxfun(@times, gradFromSpeech, future_grad_speech);
gradFromSpeech = squeeze(sum(gradFromSpeech))';
gradFromSpeech = bsxfun(@times, gradFromSpeech, 1./mask_f);

gradFromNoise = bsxfun(@minus, xx2, noiseCov);
gradFromNoise = bsxfun(@times, gradFromNoise, future_grad_noise);
gradFromNoise = squeeze(sum(gradFromNoise))';
gradFromNoise = bsxfun(@times, gradFromNoise, 1./(T-mask_f));

grad = gradFromSpeech - gradFromNoise;
% we may get complex valued gradient for real valued mask, as the partial
% derivative formula does not know that the mask must be real valued. To
% preserve the real-valued property of the mask, we just take the real part
% of the gradient, which represents the search direction of mask in the
% real domain.
grad = real(grad);

end


classdef GradientVariationWithBoundaryComputer < handle

    properties (Access = public)
       error
       mean
       desv
    end

    properties (Access = private)
        levelSet
        curvature
        rPerimeter
        backgroundMesh
        boundaryMesh
        domainLength
        circleCase
        filePlotName
        circleMesh
        unfittedMesh
        integrator

        gradientCircunf
        nEpsilon
    end

    methods (Access = public)

        function obj = GradientVariationWithBoundaryComputer(cParams)
            obj.init(cParams)
        end

        function compute(obj)
            obj.createIntegrator();
            for iEpsilon = 1:obj.nEpsilon
                obj.computeGradientInCircunference(iEpsilon);
                obj.computeVariables(iEpsilon);
            end
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.filePlotName   = cParams.filePlotName;
            obj.backgroundMesh = cParams.backgroundMesh;
            obj.circleMesh     = cParams.circleMesh;
            obj.levelSet       = cParams.levelSet;
            obj.curvature      = cParams.curvature;
            obj.rPerimeter     = cParams.regularizedPerimeter;
            obj.domainLength   = cParams.domainLength;
            obj.circleCase     = cParams.circleCase;
            obj.nEpsilon       = size(obj.rPerimeter.epsilons,2);
        end

        function computeVariables(obj,iepsilon)
           int = obj.integrator;
           M = int.computeLHS();
           k = obj.curvature;
           g = obj.gradientCircunf;
           obj.error(iepsilon) = (g-k)'*M*(g-k)/(k*sum(M(:))*k);
           obj.mean(iepsilon) = sum(int.integrate(g))/sum(int.integrate(ones(size(g))));
           obj.desv(iepsilon) = (g-obj.mean(iepsilon))'*M*(g-obj.mean(iepsilon))/(sum(M(:)));
        end

        function createIntegrator(obj)
           m = obj.circleMesh.mesh;
           m = m.computeCanonicalMesh();
           s.type = 'SIMPLE';
           s.mesh = m;
           s.npnod = m.nnodes;
           s.globalConnec = m.connec;
           obj.integrator = Integrator.create(s);
        end

        function computeGradientInCircunference(obj,iepsilon)
            nCell = obj.circleMesh.mesh.nelem;
            gB = zeros(nCell,1);
            gD = obj.rPerimeter.perimetersGradient(:,iepsilon);
            switch obj.circleCase
                case 'interior'
                    for icell = 1:nCell
                        gNodalInCell  = obj.computeFnodalInCell(icell,gD);
                        xPos          = obj.computeXpos(icell);
                        gInXpos       = obj.interpolateValue(xPos,gNodalInCell);
                        gB(icell) = gInXpos;
                    end
                case 'exterior'
                    m = obj.circleMesh;
                    gB = gD(m.nodesInBoxFaces,1);
            end
            obj.gradientCircunf = gB;
        end

        function fNodalInCell = computeFnodalInCell(obj,icell,fNodal)
            m = obj.circleMesh;
            cellGlobal   = m.cellContainingSubcell(icell);
            nodeTriangle = obj.backgroundMesh.connec(cellGlobal,:);
            fNodalInCell = fNodal(nodeTriangle(:),:);
        end

        function x1pos = computeXpos(obj,icell)
            m = obj.circleMesh;
            x1pos = m.xCoordsIso(:,1,icell);
            x1pos = squeeze(x1pos);
        end

        function fInterp = interpolateValue(obj,xpos,fNodal)
            interp = Interpolation.create(obj.backgroundMesh,'LINEAR');
            interp.computeShapeDeriv(xpos);
            shape = interp.shape;
            fInterp = 0;
            for inode = 1:length(shape)
                fInterp = fInterp + fNodal(inode)*shape(inode);
            end
        end

    end

end


function vectors3d(varargin)
%VECTORS3D Description of functions operating on 3D vectors
%
%   Vectors are represented by their 3 Cartesian coordinates:
%   V = [VX VY VZ];
%
%   List of vectors are represented by N-by-3 arrays, with the coordinates
%   of each vector on a row.
%
%
%   See also
%   vectorNorm3d, normalizeVector3d, crossProduct3d, vectorAngle3d
%   isParallel3d, isPerpendicular3d, createTranslation3d
%   drawVector3d
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2008-10-13,    using Matlab 7.4.0.287 (R2007a)
% Copyright 2008 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.


function test6
%TEST6 test for BTF
% Requires UFget
% Example:
%   test6
% See also btf, maxtrans, strongcomp, dmperm, UFget,
%   test1, test2, test3, test4, test5.

% Copyright 2007, Timothy A. Davis, University of Florida

quick2 = [ ...
 1522 -272  1463  1521   460 1507  -838 1533 -1533 -1456 -1512   734   211 ...
 -385 -735   394  -397  1109 -744  ...
 -734 -375 -1200 -1536  -837  519  -519  520  -520   189  -189   454   385 ...
  387 -387   384  -384   386 -386   388 -388   525  -525   526  -526   735 ...
 1508  209   210  1243 -1243 1534  -840 1234 -1234   390  -390   392  -392 ...
 -394 1472  1242 -1242   389 -389   391 -391   393  -393  1215 -1215  1216 ...
-1216  736  -736   737  -737  455  -455 -224  -839  1426 -1426 -1473   396 ...
 -396  398  -398   400  -400  402  -402  404  -404 -1531   395  -395   397 ...
  399 -399   401  -401   403 -403   405 -405  -738  -739  1459 -1459  1111 ...
 1110  376  -376   284  -284 -740  -742 -741  -743  1293 -1293   452   920 ...
 -745 -446  1462 -1461   448 -448   283 -283  1502 -1502  1292 -1292  1503 ...
-1503 1291 -1291   445  -445 -746  -747 1300 -1300   435  -435 -1343 -1345 ...
-1344 1305 -1305   921 -1513 1307 -1307 1369 -1369  1374 -1374  1377 ...
-1377  748  -748  -749  1510  922  -922 ] ;

index = UFget ;
nmat = length (quick2) ;
dopause = 0 ;

h = waitbar (0, 'BTF test 6 of 6') ;

try

    for k = 1:nmat

        waitbar (k/nmat, h) ;

        i = quick2 (k) ;
        Prob = UFget (abs (i), index) ;
        disp (Prob) ;
        if (i < 0)
            fprintf ('transposed\n') ;
            A = Prob.A' ;
            [m n] = size (A) ;
            if (m == n)
                if (nnz (spones (A) - spones (Prob.A)) == 0)
                    fprintf ('skip...\n') ;
                    continue ;
                end
            end
        else
            A = Prob.A ;
        end

        tic
        [p1,q1,r1,work1] = btf (A) ;
        t1 = toc ;
        n1 = length (r1) - 1 ;
        m1 = nnz (diag (A (p1, abs (q1)))) ;

        limit = work1/nnz(A) ;

        fprintf ('full search: %g * nnz(A)\n', limit) ;

        works = linspace(0,limit,9) ;
        works (1) = eps ;
        nw = length (works) ;

        T2 = zeros (nw, 1) ;
        N2 = zeros (nw, 1) ;
        M2 = zeros (nw, 1) ;

        T2 (end) = t1 ;
        N2 (end) = n1 ;
        M2 (end) = m1 ;

        fprintf ('full time %10.4f   blocks %8d  nnz(diag) %8d\n\n', t1, n1, m1) ;

        subplot (3,4,4) ;
        drawbtf (A, p1, abs (q1), r1) ;
        title (Prob.name, 'Interpreter', 'none') ;

        for j = 1:nw-1

            maxwork = works (j) ;

            tic
            [p2,q2,r2,work2] = btf (A, maxwork) ;
            t2 = toc ;
            n2 = length (r2) - 1 ;
            m2 = nnz (diag (A (p2, abs (q2)))) ;
            T2 (j) = t2 ;
            N2 (j) = n2 ;
            M2 (j) = m2 ;

            fprintf ('%9.1f %10.4f   blocks %8d  nnz(diag) %8d\n', ...
                maxwork, t2, n2, m2) ;

            subplot (3,4,4+j) ;
            drawbtf (A, p2, abs (q2), r2) ;
            title (sprintf ('%g', maxwork)) ;

            ss = [1:j nw] ;

            subplot (3,4,1) ;
            plot (works(ss), T2(ss), 'o-') ;  title ('time vs work') ;
            axis ([0 limit 0 max(0.1,max(T2))]) ;

            subplot (3,4,2) ;
            plot (works(ss), N2(ss), 'o-') ; title ('blocks vs work') ;
            axis ([0 limit 0 n1]) ;

            subplot (3,4,3) ;
            plot (works(ss), M2(ss), 'o-') ; title ('nnz(diag) vs work') ;
            axis ([0 limit 0 m1]) ;
            drawnow

        end
        fprintf ('full time %10.4f   blocks %8d  nnz(diag) %8d\n', t1, n1, m1) ;

        if (dopause)
            input ('hit enter: ') ;
        end

    end

catch
    % out-of-memory is OK, other errors are not
    disp (lasterr) ;
    if (isempty (strfind (lasterr, 'Out of memory')))
        error (lasterr) ;                                                   %#ok
    else
        fprintf ('test terminated early, but otherwise OK\n') ;
    end
end

close (h) ;


function timespderiv(options, flags)
% TIMESPDERIV   Measure the performance of SPINTERP with derivatives
%    TIMESPDERIV(OPTIONS)  Script file to plot a graph showing the
%    ratio of computing BOTH interpolated values AND gradients to
%    computing interpolated values ONLY (at 100 points simultaneously).
%    By default, the Clenshaw-Curtis-grid with sparse indices data is
%    used. Alternatively, the Chebyshev grid and other options
%    may be selected using an OPTIONS structure created with SPSET.
%
%    TIMESPDERIV(OPTIONS, FLAGS)  FLAGS is an optional struct that
%    customizes the plots.
%    The following FLAGS are available:
%       continuousDerivatives [ 'on' | 'off' ]  turns on the augmented
%         continuous derivatives available for the Clenshaw-Curtis
%         sparse grid.
%       absoluteTime [ 'on' | 'off' ]  plots the absolute measured
%         time in seconds instead of the ratio.
%       nPoints [ integer {100} ]  can be used to change the default
%         number of interpolated points.
%
%  Examples:
%    Plot ratios for CC-Grid, discontinuous derivatives:
%       timespderiv;
%
%    Plot ratios for CC-Grid, augmented continuous derivatives:
%       flags = struct('continuousDerivatives', 'on');
%       timespderiv([], flags);
%
%    Plot ratios for CGL-Grid:
%       timespderiv(spset('GridType','Chebyshev'));
%
%    Plot absolute time measurements for CGL-Grid:
%       flags = struct('absoluteTime', 'on');
%       timespderiv(spset('GridType','Chebyshev'), flags);
%
%    See also SPSET, SPINTERP, TESTFUNCTIONS.

% Author : Andreas Klimke, Universitaet Stuttgart
% Version: 1.0
% Date   : July 8, 2006

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

if nargin < 1, options = []; end
if nargin < 2, flags = []; end

continuousDerivatives = 0;
if isfield(flags, 'continuousDerivatives')
	if strcmpi(flags.continuousDerivatives, 'on')
		continuousDerivatives = 1;
	end
end

absoluteTime = 0;
if isfield(flags, 'absoluteTime')
	if strcmpi(flags.absoluteTime, 'on')
		absoluteTime = 1;
	end
end

% Number of points to interpolate
nsamplepoints = 100;
if isfield(flags, 'nPoints')
	nsamplepoints = flags.nPoints;
end

sparseIndices = spget(options, 'SparseIndices', 'auto');
gridtype = spget(options, 'GridType', 'Clenshaw-Curtis');

% Set the problem dimensions and the maximum discretization levels
% for each dimension.
switch lower(gridtype)
 case 'clenshaw-curtis'
	d = [1,2,4,8,16];
  n = [17,13,10,6,4];
 case 'noboundary'
	error('MATLAB:timespderiv:badopt',['Computing derivatives not ' ...
	      'supported for grid type ''' gridtype '''.']);
 case 'maximum'
	error('MATLAB:timespderiv:badopt',['Computing derivatives not ' ...
	      'supported for grid type ''' gridtype '''.']);
 case 'chebyshev'
	d = [1,2,4,8,16];
  n = [17,13,10,6,4];
 otherwise
	error('MATLAB:timespderiv:badopt','Unknown grid type.');
end

ndims = length(n);
markers = ['s', '.', 'd', '+', 'o', 'x', '*', 'v', '^'];

%  Compute random constants w and c for Gerz' test functions.
w = rand(d(end),1);
c = rand(d(end),1);
sumc = sum(c,1);
sumw = sum(w,1);
c = 1.5.*c/sumc;
w = w/sumw;

time = zeros(ndims,n(1)+1);
npoints = zeros(ndims,n(1)+1);

for k = 1:d(end)
	v{k} = rand(nsamplepoints,1);
end

for m = 1:ndims
	disp(['Current dim: ' num2str(d(m))]);
	z = [];
	for l = 0:n(m)
		disp(['Current level n = ' num2str(l) '...']);
		options = spset('MinDepth',l,'MaxDepth',l,'Vectorized','on',...
										'GridType', gridtype, 'SparseIndices', ...
										sparseIndices, 'PrevResults', z);
		z = spvals('testfunctions',d(m),[],options,1,c(1:d(m)),w(1: ...
																									d(m)));
		t = z.surplusCompTime;
		disp(['Computing sparse grid points and evaluating function took ' ...
					num2str(z.fevalTime) ' [s].']);
		disp(['Computing hierarchical surpluses took ' num2str(t) ' [s].']);
		npoints(m,l+1) = z.nPoints;
		if continuousDerivatives
			z.continuousDerivatives = 'on';
		end
		t = 0; k = 0;
		tic;
		while t < 0.5
			y = spinterp(z,v{1:d(m)});
			t = toc;
			k = k + 1;
		end
		t = t / k;
		t2 = 0; k = 0;
		tic;
		while t2 < 0.5
			[y,dy] = spinterp(z,v{1:d(m)});
			t2 = toc;
			k = k + 1;
		end
		t2 = t2 / k;
		if absoluteTime
			time(m,l+1) = t2;
		else
			time(m,l+1) = t2/t;
		end
		disp([num2str(nsamplepoints) ...
					' evals took ' num2str(t) ' [s], with gradients ' ...
					num2str(t2) ' [s]. Ratio: ' num2str(t2/t)]);
		disp(' ');
	end
	disp('Done!');
	disp(' ');
end

% Plot results
clf;
h = loglog(npoints',time','LineWidth',1);
for k = 1:ndims
	set(h(k), 'Marker', markers(k));
	hold on;
end
axis tight;
hold on;

% Add legend and axis labels
hold off;
if absoluteTime
	if ~continuousDerivatives
		title(['Time to compute ' num2str(nsamplepoints) ...
					' values with gradients [' gridtype ' grid].']);
	else
		title(['Time to compute ' num2str(nsamplepoints) ...
					' vals with CONTINUOUS grads [' gridtype ' grid].']);
	end
	ylabel('Time [s]')
else
	if ~continuousDerivatives
		title(['Ratio of interpolating ' num2str(nsamplepoints) ...
					' values with/without gradients [' gridtype ' grid].']);
	else
		title(['Ratio of interpol. ' num2str(nsamplepoints) ...
					' vals with/without CONTINUOUS grads [' gridtype ' grid].']);
	end
	ylabel('Ratio [-]')
	set(gca,'YTick',[1,2,5,10,20,50]);
	set(gca,'YLim',[0.8,50]);
end
xlabel('number of nodes N')
s = {};
for k = 1:ndims
	s{k} = ['d = ' num2str(d(k))];
end

if strcmpi(gridtype, 'chebyshev')
	legend(s,2);
else
	legend(s,1);
end


function Im=RGB_YUV(Y,U,V)
% This program transform YUV layers to RGB Layers in ome matrix 'Im'....
%  By   Mohammed Mustafa Siddeq
%  Date 25/7/2010

G=round((Y-(U+V)/4));
R=U+G;
B=V+G;
Im(:,:,1)=R; Im(:,:,2)=G; Im(:,:,3)=B;
%imshow(uint8(Im));
end


% extract the boundary vertices of all ROIs
% NOTE: requires Matlab R2014b

%%
% load_data;

%%
tic

figure;
xyz_ls=plot_ROI_3D(CONST,1:nCells);
close(gcf);

s=0.7; % shrink parameter, not affecting much
P=xyz_ls';
P(:,2)=-P(:,2); % special treatment for y
k_ls= boundary(P,s);

display('time to generate boundary:')
toc

%%
save(['data/boundary_k_s',num2str(s),'.mat'],'k_ls','s');
save('data/xyz_ls.mat','xyz_ls');


% C = FindClustermeans(gIX,M);
% [C_trialAvr,C_trialRes,C_score,C_d2var_perstim] = GetTrialAvrLongTrace(hGUI,C);

%% Init load
hfig = figure;
InitializeAppData(hfig);
ResetDisplayParams(hfig);

%% Load Fish8
i_fish = 8;
[cIX_load,gIX_load,M,stim,behavior,M_0] = LoadSingleFishDefault(i_fish,hfig,[1,1]);

fishset = getappdata(hfig,'fishset');
stim = getappdata(hfig,'stim');
behavior = getappdata(hfig,'behavior');
fpsec = getappdata(hfig,'fpsec');

%% crop
cropIX = 3001:3600;
stim_crop = stim(cropIX);
behavior_crop = behavior(:,cropIX);%(2,3001:3600);

% stim z
regressors_s = GetStimRegressor(stim_crop,fishset,i_fish);
reg_s = regressors_s(8).im;
% motor
regressors_m = GetMotorRegressor(behavior_crop);
reg_m = regressors_m(2).im; %1
reg_motor_n = reg_m/sqrt(sum(reg_m.*reg_m));
%%
reg_stim_n = reg_s/sqrt(sum(reg_s.*reg_s));
%%

%% get tRes/tAvr
C = reg_motor_n;
% GetTrialAvrLongTrace
period = 150; % 120 for PT, 150 for OMR
C_3D_0 = reshape(C,size(C,1),period,[]);

%     C_3D = zscore(C_3D_0,0,2);
%     C_d2var_perstim = nanmean(nanstd(C_3D,0,3),2);
%     C_score = C_d2var_perstim;

C_period = mean(C_3D_0,3);%prctile(C_3D_0,20,3);%mean(C_3D_0,3);
nPeriods = round(size(C,2)/period);
C_trialAvr = repmat(C_period,1,nPeriods);

C_trialRes = C-C_trialAvr;

%% tRes/tAvr demo trace plot
xv = (1:600)/fpsec;

% find vertical stim time-references
x_stim = stim_crop';
x_stim(x_stim~=11) = 9;
xlines = xv(find(diff([0;x_stim])~=0));

figure('Position',[50,200,300,200]); hold on

% plot vertical reference shadings
for i = 2:2:length(xlines)
    x_ = [xlines(i),xlines(i+1)];
    y_ = [-1,0.2];
    x = [x_(1),x_(2),x_(2),x_(1)];
    y = [y_(1),y_(1),y_(2),y_(2)];
   patch(x, y, [1,0.9,0.9],'Edgecolor','w');
end

% plot traces
plot(xv,reg_stim_n,'k')

yoffset = -0.3;
plot(xv,reg_motor_n+yoffset,'k')

yoffset = -0.6;
plot(xv,C_trialAvr+yoffset,'k')

yoffset = -0.9;
plot(xv,C_trialRes+yoffset,'k')

xlim([0,300])
% xlabel('Time (sec)')
ax = gca;
ax.YTick = [-0.9,-0.6,-0.3,0];
ax.YTickLabel = {'trial-Res.','trial-Avr.','motor','stim.'};

ylim([-1.2,0.2]);
set(gca,'xcolor','w','xtick',[]);
set(gca,'TickLength',[0,0]);
% plot scale bar
plot([xv(1)+5,xv(40)+5],[-1.1,-1.1],'k','linewidth',1.5);
text(xv(1)+5,-1.2,'20 sec')

%% figS3h: L/R demo trace plot
i_fish = 2;
[cIX_load,gIX_load,M,stim,behavior,M_0] = LoadSingleFishDefault(i_fish,hfig,[1,1]);

fishset = getappdata(hfig,'fishset');
stim = getappdata(hfig,'stim');
behavior = getappdata(hfig,'behavior');
fpsec = getappdata(hfig,'fpsec');

%% crop
cropIX = 3001:3300;%3001:3600;
stim_crop = stim(cropIX);
behavior_crop = behavior(:,cropIX);%(2,3001:3600);
regressors_s = GetStimRegressor(stim_crop,fishset,i_fish);
regressors_m = GetMotorRegressor(behavior_crop);

reg_mL = regressors_m(1).im; %1
reg_mR = regressors_m(2).im; %1
reg_mL_n = reg_mL/sqrt(sum(reg_mL.*reg_mL));
reg_mR_n = reg_mR/sqrt(sum(reg_mR.*reg_mR));
lrAvr = 0.5*(reg_mL_n+reg_mR_n);
lrAvr_n = lrAvr/sqrt(sum(lrAvr.*lrAvr));
lrRes1 = reg_mL_n-lrAvr;
lrRes_n1 = lrRes1/sqrt(sum(lrRes1.*lrRes1));
lrRes2 = reg_mR_n-lrAvr;
lrRes_n2 = lrRes2/sqrt(sum(lrRes2.*lrRes2));
%%
figure('Position',[50,200,400,300]); hold on
xv = (1:length(cropIX))/fpsec;

% plot vertical reference shadings
% for i = 2:2:length(xlines)
%     x_ = [xlines(i),xlines(i+1)];
%     y_ = [-1,0.2];
%     x = [x_(1),x_(2),x_(2),x_(1)];
%     y = [y_(1),y_(1),y_(2),y_(2)];
%    patch(x, y, [1,0.9,0.9],'Edgecolor','w');
% end

% plot traces
% plot(xv,reg_stim_n,'k')
spacing = 0.5;
yoffset = 0;
plot(xv,reg_mL_n+yoffset,'k')

yoffset = yoffset-spacing;
plot(xv,reg_mR_n+yoffset,'k')

yoffset = yoffset-spacing;
plot(xv,lrAvr_n+yoffset,'k')

yoffset = yoffset-spacing;
plot(xv,lrRes_n1+yoffset,'k')

yoffset = yoffset-spacing;
plot(xv,lrRes_n2+yoffset,'k')

% xlim([0,300])
% xlabel('Time (sec)')
ax = gca;
ax.YTick = [-spacing*4,-spacing*3,-spacing*2,-spacing,0];
ax.YTickLabel = {'R-Res.','L-Res.','LR-Avr.','R motor','L motor'};

ylim([-spacing*5,spacing]);
set(gca,'xcolor','w','xtick',[]);
set(gca,'TickLength',[0,0]);
% plot scale bar
plot([xv(1)+5,xv(40)+5],[-spacing*4.6,-spacing*4.6],'k','linewidth',1.5);
text(xv(1)+5,-spacing*4.9,'20 sec')


function showmesh3(node,elem,expr,varargin)
%% SHOWMESH3 displays a tetrahedron mesh in 3-D.
%
%    showmesh3(node,elem) displays a 3-dimensional tetrahderon mesh given
%    by node and elem matrices; see meshdoc for the data structure:
%    node and elem.
%
%    showmesh3(node,elem,expr) displays parts of the mesh specificed by the
%    expression. For example, showmesh3(node,elem,'~(x>=0 & y>=0)') only
%    shows the tetrahedron not in the first quadrant.
%
%    showmesh3(node,elem,expr,'param','value','param','value'...) allows
%    additional patch param/value pairs to be used when displaying the
%    mesh. For example, the default transparency parameter is set to 0.5.
%    You can overwrite this value by using the param pair ('FaceAlpha',
%    value). The value has to be a number between 0 and 1. Other parameters
%    include: 'Facecolor', 'Edgecolor' etc.
%
%    For meshes with large data, the 3-D graphics is very slow. You may use
%    showboundary3 to display the boundary surface mesh only.
%
%   Example:
%     % A mesh for a cube
%     node = [-1,-1,-1; 1,-1,-1; 1,1,-1; -1,1,-1; -1,-1,1; 1,-1,1; 1,1,1; -1,1,1];
%     elem = [1,2,3,7; 1,6,2,7; 1,5,6,7; 1,8,5,7; 1,4,8,7; 1,3,4,7];
%     [node,elem] = uniformbisect3(node,elem);
%     subplot(1,2,1);
%     showmesh3(node,elem); pause(1)
%     subplot(1,2,2);
%     showmesh3(node,elem,'~(x>=0 & y>=0 & z>=0)','FaceAlpha',0.25);
%     axis on; view([59,20])
%
%   See also showboundary3, showsolution3, showmesh.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if (nargin >= 3) && (any(expr))
    x = node(:,1);  y = node(:,2);  z = node(:,3); %#ok<NASGU>
    incl = find(eval(expr));
    elem = elem(any(ismember(elem,incl),2),:);
end
h = tetramesh(elem(:,1:4),node,ones(size(elem,1),1));
if nargin > 3
    set(h,varargin{1:end})
else % default display properties
    set(h,'FaceAlpha',0.4);
end
view(3);
axis off; axis equal; axis tight


function [H, Z, S, psi] = sfactorization_wilson3x3(S,freq,Niterations,tol,cmbindx,fb,init,checkflag,stabilityfix)

% SFACTORIZATION_WILSON3X3 performs triplet-wise non-parametric spectral factorization on
% cross-spectra, based on Wilson's algorithm.
%
% Usage  : [H, Z, psi] = sfactorization_wilson(S,freq);
%
% Inputs : S (1-sided, 3D-spectral matrix in the form of Channel x Channel x frequency)
%        : freq (a vector of frequencies) at which S is given.
%
% Outputs: H (transfer function)
%        : Z (noise covariance)
%        : S (cross-spectral density 1-sided)
%        : psi (left spectral factor)
%
% This function is an implemention of Wilson's algorithm (Eq. 3.1)
% for spectral matrix factorization.
%
% Ref: G.T. Wilson,"The Factorization of Matricial Spectral Densities,"
% SIAM J. Appl. Math.23,420-426(1972).
% Written by M. Dhamala & G. Rangarajan, UF, Aug 3-4, 2006.
% Email addresses: mdhamala@bme.ufl.edu, rangaraj@math.iisc.ernet.in

% Copyright (C) 2009-2017, Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if nargin<9, stabilityfix = false; end
if nargin<8, checkflag = true;   end
if nargin<7, init      = 'chol'; end
if nargin<6, fb        = 'none'; end
if nargin<5
  ft_error('FieldTrip:connectivity:sfactorization_wilson3x3', 'when requesting multiple triplet-wise spectral decomposition, ''cmbindx'' needs to be specified');
end
if nargin<4, tol        = 1e-8;   end
if nargin<3, Niterations = 1000;  end

dfreq = round(diff(freq)*1e5)./1e5; % allow for some numeric issues
if ~all(dfreq==dfreq(1))
  ft_error('FieldTrip:connectivity:sfactorization_wilson3x3', 'frequency axis is not evenly spaced');
end

if freq(1)~=0
  ft_warning('FieldTrip:connectivity:sfactorization_wilson3x3', 'when performing non-parametric spectral factorization, the frequency axis should ideally start at 0, zero padding the spectral density');
  dfreq = mean(dfreq);
  npad  = freq(1)./dfreq;

  % update the freq axis and keep track of the frequency bins that are
  % expected in the output
  selfreq  = (1:numel(freq)) + npad;
  freq     = [(0:(npad-1))./dfreq freq];
  S        = cat(3, zeros(size(S,1), size(S,1), npad), S);
else
  selfreq  = 1:numel(freq);
end

% ensure input S is double (mex-files don't work with single)
S = double(S);

% check whether the last frequency bin is strictly real-valued.
% if that's the case, then it is assumed to be the Nyquist frequency
% and the two-sided spectral density will have an even number of
% frequency bins. if not, in order to preserve hermitian symmetry,
% the number of frequency bins needs to be odd.
Send = S(:,:,end);
N    = numel(freq);
m    = size(cmbindx,1);
if all(imag(Send(:))<abs(trace(Send)./size(Send,1)*1e-9))
  hasnyq = true;
  N2     = 2*(N-1);
else
  hasnyq = false;
  N2     = 2*(N-1)+1;
end

% preallocate memory for the identity matrix
I      = repmat(eye(3),[1 1 m N2]); % Defining 3 x 3 identity matrix

% %Step 1: Forming 2-sided spectral densities for ifft routine in matlab
% Sarr   = zeros(3,3,m,N2) + 1i.*zeros(3,3,m,N2);
% for c = 1:m
%   Stmp  = S(cmbindx(c,:),cmbindx(c,:),:);
%
%   % the input cross-spectral density is assumed to be weighted with a
%   % factor of 2 in all non-DC and Nyquist bins, therefore weight the
%   % DC-bin with a factor of 2 to get a correct two-sided representation
%   Sarr(:,:,c,1) = Stmp(:,:,1).*2;
%
%   for f_ind = 2:N
%     Sarr(:,:,c,       f_ind) = Stmp(:,:,f_ind);
%     Sarr(:,:,c,(N2+2)-f_ind) = Stmp(:,:,f_ind).';
%   end
% end
% Sarr2 = Sarr;

% preallocate memory for the 2-sided spectral density
Sarr = zeros(3,3,N2,m) + 1i.*zeros(3,3,N2,m);
for c = 1:m
  Sarr(:,:,1:N,c) = S(cmbindx(c,:),cmbindx(c,:),:);
end
if hasnyq
  N1 = N;
else
  N1 = N + 1; % the highest frequency needs to be represented twice, for symmetry purposes
end
Sarr(:,:,N1:N2,:) = flip(Sarr(:,:,2:N,:),3);
Sarr(2,1,N1:N2,:) = conj(Sarr(2,1,N1:N2,:));
Sarr(1,2,N1:N2,:) = conj(Sarr(1,2,N1:N2,:));
Sarr(3,1,N1:N2,:) = conj(Sarr(3,1,N1:N2,:));
Sarr(1,3,N1:N2,:) = conj(Sarr(1,3,N1:N2,:));
Sarr(3,2,N1:N2,:) = conj(Sarr(3,2,N1:N2,:));
Sarr(2,3,N1:N2,:) = conj(Sarr(2,3,N1:N2,:));
Sarr              = permute(Sarr, [1 2 4 3]);
Sarr(:,:,:,1)     = Sarr(:,:,:,1).*2; % weight the DC-bin

% the input cross-spectral density is assumed to be weighted with a
% factor of 2 in all non-DC and Nyquist bins, therefore weight the
% Nyquist bin with a factor of 2 to get a correct two-sided representation
if hasnyq
  Sarr(:,:,:,N) = Sarr(:,:,:,N).*2;
end

%Step 2: Computing covariance matrices
gam = real(reshape(ifft(reshape(Sarr, [9*m N2]), [], 2),[3 3 m N2]));

%Step 3: Initializing for iterations
gam0 = gam(:,:,:,1);

h    = complex(zeros(size(gam0)));
for k = 1:m
  switch init
    case 'chol'
      [tmp, dum] = chol(gam0(:,:,k));
      if dum
        ft_warning('initialization with ''chol'' for iterations did not work well, using arbitrary starting condition');
        tmp = rand(3,3); %arbitrary initial condition
        tmp = triu(tmp);
      end
    case 'rand'
      tmp = rand(3,3); %arbitrary initial condition
      tmp = triu(tmp);
    otherwise
      ft_error('initialization method should be eithe ''chol'' or ''rand''');
  end
  h(:,:,k) = tmp;

  %h(:,:,k) = chol(gam0(:,:,k));
end
psi  = repmat(h, [1 1 1 N2]);

%Step 4: Iterating to get spectral factors
ft_progress('init', fb, 'computing spectral factorization');
for iter = 1:Niterations
  ft_progress(iter./Niterations, 'computing iteration %d/%d\n', iter, Niterations);
  invpsi = inv3x3(psi);
  g      = sandwich3x3(invpsi, Sarr) + I;
  gp     = PlusOperator3x3(g,m,N,stabilityfix); %gp constitutes positive and half of zero lags

  psi_old = psi;
  psi     = mtimes3x3(psi, gp);

  if checkflag
    psierr  = abs(psi-psi_old)./abs(psi);
    psierrf = mean(psierr(:));
    if(psierrf<tol)
      fprintf('reaching convergence at iteration %d\n',iter);
      break;
    end % checking convergence
  end
end
ft_progress('close');

%Step 5: Getting covariance matrix from spectral factors
gamtmp = reshape(real(ifft(transpose(reshape(psi, [9*m N2]))))', [3 3 m N2]);

%Step 6: Getting noise covariance & transfer function (see Example pp. 424)
%
A0    = gamtmp(:,:,:,1);
A0inv = inv3x3(A0);

Z = zeros(3,3,m);
for k = 1:m
  %Z     = A0*A0.'*fs; %Noise covariance matrix
  Z(:,:,k) = A0(:,:,k)*A0(:,:,k).'; %Noise covariance matrix not multiplied by sampling frequency
  %FIXME check this; at least not multiplying it removes the need to correct later on
  %this also makes it more equivalent to the noisecov estimated by biosig's mvar-function
end

% H = complex(zeros(3,3,m,N));
% S = complex(zeros(3,3,m,N));
% for k = 1:N
%   for kk = 1:m
%     H(:,:,kk,k) = psi(:,:,kk,k)*A0inv(:,:,kk);  % Transfer function
%     S(:,:,kk,k) = psi(:,:,kk,k)*psi(:,:,kk,k)'; % Cross-spectral density
%   end
% end
H = mtimes3x3(psi,A0inv(:,:,:,ones(1,size(psi,4))));
S = mtimes3x3(psi,ctranspose3x3(psi));

siz = [size(H) 1 1];
H   = reshape(H, [9*siz(3) siz(4:end)]);
siz = [size(S) 1 1];
S   = reshape(S, [9*siz(3) siz(4:end)]);
siz = [size(Z) 1 1];
Z   = reshape(Z, [9*siz(3) siz(4:end)]);
siz = [size(psi) 1 1];
psi = reshape(psi, [9*siz(3) siz(4:end)]);

%if numel(selfreq)~=numel(freq)
  % return only the frequency bins that were in the input
  H   =   H(:,selfreq,:,:);
  S   =   S(:,selfreq,:,:);
  psi = psi(:,selfreq,:,:);
%end

%---------------------------------------------------------------------
function gp = PlusOperator3x3(g,ncmb,nfreq, stabilityfix)

% This function is for [ ]+operation:
% to take the positive lags & half of the zero lag and reconstitute
% M. Dhamala, UF, August 2006

g   = transpose(reshape(g, 9*ncmb, []));
gam = ifft(g);

% taking only the positive lags and half of the zero lag
gamp  = gam;
beta0 = 0.5*gam(1,:);

%for k = 1:ncmb
%  gamp(1,(k-1)*4+1:k*4) = reshape(triu(reshape(beta0(1,(k-1)*4+1:k*4),[2 2])),[1 4]);
%end
beta0(2:9:9*ncmb)   = 0;
beta0(3:9:9*ncmb)   = 0;
beta0(6:9:9*ncmb)   = 0;
gamp(1,:)           = beta0;
gamp(nfreq+1:end,:) = 0;

% smooth with a window, only for the long latency boundary: this is a
% stabilityfix proposed by Martin Vinck
if stabilityfix
  w = tukeywin(nfreq*2, 0.5);
  gamp(1:nfreq,:) = gamp(1:nfreq,:).*repmat(w(nfreq+1:end),[1 nchan^2]);
else
  % nothing to be done here
end

% reconstituting
gp = fft(gamp);
gp = reshape(transpose(gp), [3 3 ncmb numel(gp)/(9*ncmb)]);


function C = cluster_rotation_classes(vecs, k, vis)

styles = get_vis_styles();

[idx, C]= kmeans(vecs,k);

if vis
    figure(1);
    hold on;

    % do PCA to display stuff
    if size(vecs, 2) > 2
        [coeff,score,latent,tsquared,explained,mu] = pca(vecs);
        explained
        vecs = score;
    end

    for i = 1:k
        iset = (idx == i);
        if size(vecs, 2) > 2
            scatter3(vecs(iset, 1), vecs(iset, 2), vecs(iset, 3));
        else
            scatter(vecs(iset, 1), vecs(iset, 2), styles{i});
        end
    end
end


function pass = test_constructor( pref )
% Test the Chebfun2v constructor when performing simple arithmetic
% operations.

if ( nargin < 1 )
    pref = chebfunpref;
end

tol = 100*pref.cheb2Prefs.chebfun2eps;

D = [-1 1 -1 1; -2 3 0 1];

for kk = 1:size(D,1)
    d = D(kk,:);
    % Check the constructor works with lots of different syntax:
    f1 = @(x,y) cos(x.*y);
    f2 = @(x,y) sin(x) + cos(y);
    f3 = @(x,y) exp(x) - exp(-y) + x;
    g1 = chebfun2(f1, d);
    g2 = chebfun2(f2, d);
    g3 = chebfun2(f3, d);

    H1 = chebfun2v(f1, f2, d);
    H2 = chebfun2v(f1, g2, d);
    H3 = chebfun2v(g1, f2, d);
    H4 = chebfun2v(g1, g2, d);

    pass(1) = norm( H1 - H2 ) < tol;
    pass(2) = norm( H1 - H3 ) < tol;
    pass(3) = norm( H1 - H4 ) < tol;

    % Now try the same thing for three components:
    H1 = chebfun2v(f1, f2, f3, d);
    H2 = chebfun2v(f1, f2, g3, d);
    H3 = chebfun2v(f1, g2, f3, d);
    H4 = chebfun2v(g1, f2, f3, d);
    H5 = chebfun2v(g1, g2, f3, d);
    H6 = chebfun2v(g1, f2, g3, d);
    H7 = chebfun2v(f1, g2, g3, d);
    H8 = chebfun2v(g1, g2, g3, d);

    pass(4) = norm( H1 - H2 ) < tol;
    pass(5) = norm( H1 - H3 ) < tol;
    pass(6) = norm( H1 - H4 ) < tol;
    pass(7) = norm( H1 - H5 ) < tol;
    pass(8) = norm( H1 - H6 ) < tol;
    pass(9) = norm( H1 - H7 ) < tol;
    pass(10) = norm( H1 - H8 ) < tol;

end

end


function pstruct = tapas_kf_namep(pvec)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2016 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

pstruct = struct;

pstruct.g_0       = pvec(1);
pstruct.mu_0      = pvec(2);
pstruct.om        = pvec(3);
pstruct.pi_u      = pvec(4);

return;


% MANIPULATOR TRAJECTORY GENERATION
% Generates Joint space trajectories by performing inverse kinematics on
% each waypoint and interpolating between the joint angles.
%
% Copyright 2019 The MathWorks, Inc.

%% Setup
clear, clc, close all

% Define waypoint information
createWaypointData;

% Define IK
ik = inverseKinematics('RigidBodyTree',gen3);
ikWeights = [1 1 1 1 1 1];
ikInitGuess = jointAnglesHome';
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;

% Set up plot
plotMode = 1; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames
show(gen3,gen3.homeConfiguration,'Frames','off','PreservePlot',false);
xlim([-1 1]), ylim([-1 1]), zlim([0 1.2])
hold on
if plotMode == 1
    hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-');
end
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2);

%% Solve IK for all waypoints
includeOrientation = false; % Set this to use zero vs. nonzero orientations

numWaypoints = size(waypoints,2);
numJoints = numel(gen3.homeConfiguration);
jointWaypoints = zeros(numJoints,numWaypoints);

for idx = 1:numWaypoints
    if includeOrientation
        tgtPose = trvec2tform(waypoints(:,idx)') * eul2tform(orientations(:,idx)');
    else
        tgtPose =  trvec2tform(waypoints(:,idx)');
    end
    [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
    jointWaypoints(:,idx) = config';
end

%% Generate trajectory on joint space
trajType = 'trap';
switch trajType
    case 'trap'
        [q,qd,qdd] = trapveltraj(jointWaypoints,numel(trajTimes), ...
            'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ...
            'EndTime',repmat(diff(waypointTimes),[numJoints 1]));

    case 'cubic'
        [q,qd,qdd] = cubicpolytraj(jointWaypoints,waypointTimes,trajTimes, ...
            'VelocityBoundaryCondition',zeros(numJoints,numWaypoints));

    case 'quintic'
        [q,qd,qdd] = quinticpolytraj(jointWaypoints,waypointTimes,trajTimes, ...
            'VelocityBoundaryCondition',zeros(numJoints,numWaypoints), ...
            'AccelerationBoundaryCondition',zeros(numJoints,numWaypoints));

    case 'bspline'
        ctrlpoints = jointWaypoints; % Can adapt this as needed
        [q,qd,qdd] = bsplinepolytraj(ctrlpoints,waypointTimes([1 end]),trajTimes);

    otherwise
        error('Invalid trajectory type! Use ''trap'', ''cubic'', ''quintic'', or ''bspline''');
end

% To visualize the trajectory, run the following line
% plotTrajectory(trajTimes,q,qd,qdd,'Names',"Joint " + string(1:numJoints),'WaypointTimes',waypointTimes)

%% Trajectory following loop
for idx = 1:numel(trajTimes)

    config = q(:,idx)';

    % Find Cartesian points for visualization
    eeTform = getTransform(gen3,config,eeName);
    if plotMode == 1
        eePos = tform2trvec(eeTform);
        set(hTraj,'xdata',[hTraj.XData eePos(1)], ...
                  'ydata',[hTraj.YData eePos(2)], ...
                  'zdata',[hTraj.ZData eePos(3)]);
    elseif plotMode == 2
        plotTransforms(tform2trvec(eeTform),tform2quat(eeTform),'FrameSize',0.05);
    end

    % Show the robot
    show(gen3,config,'Frames','off','PreservePlot',false);
    title(['Trajectory at t = ' num2str(trajTimes(idx))])
    drawnow

end


function d = Ddiv(A,B)
    d = sum(sum(A .* log (A./B) - A + B));
end


function test_example_SAE
load mnist_uint8;

train_x = double(train_x)/255;
test_x  = double(test_x)/255;
train_y = double(train_y);
test_y  = double(test_y);

%%  ex1 train a 100 hidden unit SDAE and use it to initialize a FFNN
%  Setup and train a stacked denoising autoencoder (SDAE)
rand('state',0)
sae = saesetup([784 100]);
sae.ae{1}.activation_function       = 'sigm';
sae.ae{1}.learningRate              = 1;
sae.ae{1}.inputZeroMaskedFraction   = 0.5;
opts.numepochs =   1;
opts.batchsize = 100;
sae = saetrain(sae, train_x, opts);
visualize(sae.ae{1}.W{1}(:,2:end)')

% Use the SDAE to initialize a FFNN
nn = nnsetup([784 100 10]);
nn.activation_function              = 'sigm';
nn.learningRate                     = 1;
nn.W{1} = sae.ae{1}.W{1};

% Train the FFNN
opts.numepochs =   1;
opts.batchsize = 100;
nn = nntrain(nn, train_x, train_y, opts);
[er, bad] = nntest(nn, test_x, test_y);
assert(er < 0.16, 'Too big error');


function DirT = transformRTdir(Dir,Rt)

DirT = Rt(1:3,1:3) * Dir;


report_this_filefun(mfilename('fullpath'));

clf
[X,Y,Z] = meshgrid(gx,gy,gz);
v = [];
i = 0.05
ii = 0.05
for i3 = 1:2:length(gx)
    i3 = gx(i3);
    l = loc(1,:) == i3;
    re3 = reshape(as(l),length(gz),length(gy));
    r = reshape(loc(4,l),length(gz),length(gy));
    l = r > tresh;
    re3(l) = zeros(1,length(find(l)))*nan;
    rect = [i ii 0.15 0.15];
    i = i+0.20;
    if i > 0.9 ; i = 0.05 ; ii = ii + 0.20; end
    axes('position',rect)
    pcolor(gy,-gz,re3)
    caxis([min(as) max(as)])
    colormap(jet)
    title([ 'Long=' num2str(i3) ' km'],'FontSize',8)
    set(gca,'FontSize',8')
    hold on
    l = a.Longitude > i3-dx/2 & a.Longitude < i3+dx/2;
    plot3(a(l,2),-a(l,7),a(l,7)*0,'k.')
    shading interp
    % axis('off')
    hold on
end
%colorbar


function poly = boxToPolygon(box)
%BOXTOPOLYGON Convert a bounding box to a square polygon.
%
%   poly = boxToPolygon(box)
%   Utility function that convert box data in [XMIN XMAX YMIN YMAX] format
%   to polygon data corresponding to the box boundary. The resulting POLY
%   is a 4-by-2 array.
%
%
%   Example
%     box = [ 10 50 20 40];
%     poly = boxToPolygon(box)
%     poly =
%         10    20
%         50    20
%         50    40
%         10    40
%
%   See also
%     boxes2d, polygons2d, boxToRect

% ------
% Author: David Legland
% E-mail: david.legland@nantes.inra.fr
% Created: 2017-09-10, using Matlab 8.6.0.267246 (R2015b)
% Copyright 2017-2022 INRA - Cepia Software Platform

% extreme coordinates
xmin = box(1);
xmax = box(2);
ymin = box(3);
ymax = box(4);

% convert to polygon
poly = [...
    xmin ymin; ...
    xmax ymin; ...
    xmax ymax; ...
    xmin ymax];


%% Copyright (C) 2014, 2016, 2018 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy is free software; you can redistribute it and/or modify
%% it under the terms of the GNU General Public License as published
%% by the Free Software Foundation; either version 3 of the License,
%% or (at your option) any later version.
%%
%% This software is distributed in the hope that it will be useful,
%% but WITHOUT ANY WARRANTY; without even the implied warranty
%% of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See
%% the GNU General Public License for more details.
%%
%% You should have received a copy of the GNU General Public
%% License along with this software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym conj (@var{z})
%% Symbolic conjugate function.
%%
%% Examples:
%% @example
%% @group
%% syms z
%% conj(z)
%%   @result{} ans = (sym)
%%       _
%%       z
%%
%% @end group
%% @group
%% syms x real
%% conj(x)
%%   @result{} ans = (sym) x
%%
%% conj(sym(pi) + 6i)
%%   @result{} ans = (sym) π - 6⋅ⅈ
%% @end group
%% @end example
%%
%% Unlike @ref{@@sym/ctranspose}, this command does not transpose
%% a matrix:
%% @example
%% @group
%% A = [1 z x; sym(4) 5 6+7i]
%%   @result{} A = (sym 2×3 matrix)
%%       ⎡1  z     x   ⎤
%%       ⎢             ⎥
%%       ⎣4  5  6 + 7⋅ⅈ⎦
%% conj(A)
%%   @result{} ans = (sym 2×3 matrix)
%%       ⎡   _         ⎤
%%       ⎢1  z     x   ⎥
%%       ⎢             ⎥
%%       ⎣4  5  6 - 7⋅ⅈ⎦
%% @end group
%% @end example
%%
%% @seealso{@@sym/ctranspose, @@sym/real, @@sym/imag}
%% @end defmethod

function z = conj(x)

  if (nargin ~= 1)
    print_usage ();
  end

  % can just use .conjugate on matrix but avoids S.true err
  sf = { 'def _op(x):'
         '    if x in (S.true, S.false):'
         '        return x'
         '    return x.conjugate()' };

  z = elementwise_op (sf, x);

end

%!test
%! a = sym(6);
%! b = sym(5i);
%! assert (isequal (conj(a), a))
%! assert (isequal (conj(b), -b))
%! assert (isequal (conj(a+b), a-b))

%!test
%! syms x
%! assert (isequal (conj(conj(x)), x))

%!test
%! syms x real
%! assert (isequal (conj(x), x))

%!test
%! % array
%! syms x
%! A = [x 6+1i; sym(1) x+2i];
%! B = [conj(x) 6-1i; sym(1) conj(x)-2i];
%! assert (isequal (conj(A), B))

%!test
%! % true/false
%! t = sym(true);
%! f = sym(false);
%! assert (isequal ( conj(t), t))
%! assert (isequal ( conj(f), f))

%!test
%! % round trip
%! syms x
%! d = 3 - 5i;
%! f = conj (x);
%! A = conj (d);
%! h = function_handle (f);
%! B = h (d);
%! assert (A, B)


%fill_mm_sub Proj. reconstruction of a normed sub-scene.
%
%   When the central image concept is used, the information which image is
%   the central image is passed to this function as input.
%
%   Parameters:
%
%      Mfull .. complete known parts of the problem, used here for the best
%               estimate of the fundamental matrices

function [P,X, lambda, u1,u2, info] = fill_mm_sub(Mfull, M, central,opt,info)

I = ~isnan(M(1:3:end,:));
[m n] = size(I);
if isempty(central), central = 0; end

P=[]; X=[]; lambda=[]; u1=1:m; u2=1:n;

%estimate the fundamental matrices and epipoles with the method of [Har95]
 [F,ep,rows,nonrows] = M2Fe(Mfull, central);

 if ~isempty(nonrows),
   disp(sprintf('Used local images:%s.', sprintf(' %d', rows))); end
 if length(rows) < 2, return; end

%determine scale faktors lambda_i_p
 if ~central, rows_central = 0; else rows_central = find(rows == central); end
 [lambda, Ilamb] = depth_estimation(M(k2i(rows),:),F,ep,rows, ...
                                    rows_central);

     % prepare info.show_prmm - for show_prmm function
     info.show_prmm.I = I;
     info.show_prmm.Idepths = zeros(m,n); info.show_prmm.Idepths(rows,:)=Ilamb;

%build the rescaled measurement matrix B
 for i = 1:length(rows), B(k2i(i),:) = M(k2i(i),:).*([1;1;1]*lambda(i,:)); end

%balance W by column-wise and "triplet-of-rows"-wise scalar multiplications
 B = balance_triplets(B, opt);

%fit holes in JIM by Jacobs' algorithm
 [P,X, u1,u2, lambda1, info] = fill_prmm(B, Ilamb, central,opt,info);

r1 = setdiff(1:length(rows),u1); r2 = setdiff(1:n,u2);

lambda = lambda(r1,r2);  % to fit P*X
if ~isempty(lambda1),
  new = find(~Ilamb(r1,r2) & I(r1,r2)); lambda(new) = lambda1(new); end

error = eucl_dist_only(B(k2i(r1), r2), P*X, ~isnan(B(3*r1,r2)), 3);
if opt.verbose, disp(sprintf('Error balanced: %f', error)); end

u1 = union(nonrows, rows(u1));


function Population = archive(Population,N)
% Select feasible and non-dominated solutions by using SPEA2-CDP

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

    %% Select feasible solutions
    fIndex     = all(Population.cons <= 0,2);
    Population = Population(fIndex);
    if isempty(Population)
        return
    elseif size(Population,2) > N
        Fitness = CalFitness(Population.objs,Population.cons);
        Next = Fitness < 1;
        Del  = Truncation(Population(Next).objs,sum(Next)-N);
        Temp = find(Next);
        Next(Temp(Del)) = false;
        Population = Population(Next);
    end
end

function Del = Truncation(PopObj,K)
% Select part of the solutions by truncation

    %% Truncation
    Distance = pdist2(PopObj,PopObj);
    Distance(logical(eye(length(Distance)))) = inf;
    Del = false(1,size(PopObj,1));
    while sum(Del) < K
        Remain   = find(~Del);
        Temp     = sort(Distance(Remain,Remain),2);
        [~,Rank] = sortrows(Temp);
        Del(Remain(Rank(1))) = true;
    end
end