%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_FLEXION_N2(robot, T)
%   Solves the inverse kinematic problem for the ABB IRB 140 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_FLEXION_N2 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   >>epson=load_robot('EPSON', 'FLEXION_N2');
%   >>q = [0 0 0 0 0 0];
%   >>T = directkinematic(epson, q);
%
%   %Call the inversekinematic for this robot
%
%   >> qinv = inversekinematic(epson, T);
%
%   check that all of them are feasible solutions!
%   and every Ti equals T
%
%   for i=1:8,
%        Ti = directkinematic(epson, qinv(:,i))
%   end
%
%	See also DIRECTKINEMATIC.
%
%   Author: Arturo Gil Aparicio
%           Universitas Miguel Hernandez, SPAIN.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_flexion_n2(robot, T)

Ts=[-1 0 0 0; 0 -1 0 0; 0 0 -1 0.9; 0 0 0 1];
T=inv(Ts)*T;
%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

%Evaluate the parameters
d = eval(robot.DH.d);

%See geometry at the reference for this robot
L6=d(6);

%A1 = a(1);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1))+pi/2;

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%Arrange solutions, there are 8 possible solutions so far.
% if q1 is a solution, q1* = q1 + pi is also a solution.
% For each (q1, q1*) there are two possible solutions
% for q2 and q3 (namely, elbow up and elbow up solutions)
% So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
% there exist two more possible solutions for the last three joints, generally
% called wrist up and wrist down solutions. For this reason,
%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    % use solve_spherical_wrist2 for the particular orientation
    % of the systems in this EPSON robot
    % use either the geometric or algebraic method.
    % the function solve_spherical_wrist2 is used due to the relative
    % orientation of the last three DH reference systems.

    %use either one algebraic method or the geometric
    qtemp = solve_spherical_wrist_flexion_n2(robot, q(:,i), T, 1, 'geometric'); %wrist up
    %qtemp = solve_spherical_wrist_flexion_n2(robot, q(:,i), T, 1,'algebraic'); %wrist up
    %qtemp = solve_spherical_wrist(robot, q(:,i), T, 1, 'geometric'); %wrist up
    %qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'algebraic'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist_flexion_n2(robot, q(:,i), T, -1, 'geometric'); %wrist down
    %qtemp = solve_spherical_wrist_flexion_n2(robot, q(:,i), T, -1,'algebraic'); %wrist up
    %qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'algebraic'); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = (acos((L2^2+r^2-L3^2)/(2*r*L2)));

if ~isreal(gamma)
    disp('WARNING:inversekinematic_flexion_n2: the point is not reachable for this configuration, imaginary solutions');
    %gamma = real(gamma);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = (acos((L2^2 + L3^2 - r^2)/(2*L2*L3)));

if ~isreal(eta)
   disp('WARNING:inversekinematic_flexion_n2: the point is not reachable for this configuration, imaginary solutions');
   %eta = real(eta);
end

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - eta;
q3(2) = eta - 3*pi/2 + pi/2;


function lambda = ill3_eigenvalues ( )

%*****************************************************************************80
%
%% ILL3_EIGENVALUES returns the eigenvalues of the ILL3 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 July 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Joan Westlake,
%    A Handbook of Numerical Matrix Inversion and Solution of
%    Linear Equations,
%    John Wiley, 1968,
%    ISBN13: 978-0471936756,
%    LC: QA263.W47.
%
%  Parameters:
%
%    Output, real LAMBDA(3,1), the eigenvalues.
%
  lambda = [ 3.0; 2.0; 1.0 ];

  return
end


function startIdx=findPatternInArray(pattern,array,maxTimes)
%%FINDPATTERNINARRAY Find all or a maximum number of occurrences of a
%                    pattern in an array of values.
%
%INPUTS: pattern A 1Xm or mX1 array holding elements of a pattern to be
%                found.
%        array   A 1Xn or nX1 array in which occurrences of the pattern are
%                to be found.
%       maxTimes An optional parameter specifying the maximum number of
%                occurrences of the pattern to find in array. If omitted or
%                an empty matrix is passed, then all occurrences of pattern
%                in array will be found.
%
%OUTPUTS: startIdx A numFoundX1 array of the atarting indices of the
%                occurrences of pattern in array, given in increasing
%                order.
%
%The algorithm used is a linear time method given in [1]. It is the
%algorithm of Section 2 with the modification to the next array of Section
%4 to find all occurrences of the pattern. Not all of the suggestions for
%improving efficiency of Section 3 are used, because they would require
%lengthening the input arrays and also would require defining certain
%values the inputs could not take.
%
%EXAMPLE:
% array='baabbabbaabaabbaabbaabbabaa';
% pattern='aabba';
% startIdx=findPatternInArray(pattern,array)
%One finds the string at startIdx=[2;12;16;20]. Note that some of the
%solutions overlap.
%
%REFERENCES:
%[1] D. E. Knuth, J. H. Morris Jr., and V. R. Pratty, "Fast pattern
%    matching in strings," SIAM Journal on Computing, vol. 6, no. 2, pp.
%    323-350, Jun. 1977.
%
%August 2015, David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

m=length(pattern);
n=length(array);

if(m>n)
    %If the pattern is longer than the array, then a match is impossible.
    startIdx=[];
    return;
end

if(nargin<3||isempty(maxTimes))
   maxTimes=n-m+1;%The maximum
else
   maxTimes=min([n-m+1,maxTimes]);
end

%Allocate space for the maximum possible number of matches.
startIdx=zeros(maxTimes,1);
numFound=0;

%First, compute the elements of the next table. We are computing them for a
%length m+1 table where the last element does not match anything else so
%that we can get a resume index to use when finding multiple matches.
next=zeros(m+1,1);
j=1;
t=0;
next(1)=0;
while(j<m)
   while((t>0)&&pattern(j)~=pattern(t))
       t=next(t);
   end
   t=t+1;
   j=j+1;
   if(pattern(j)==pattern(t))
       next(j)=next(j);
   else
        next(j)=t;
   end
end

%Add in the value for j=m;
while((t>0)&&pattern(j)~=pattern(t))
   t=next(t);
end
t=t+1;
j=j+1;
next(j)=t;%This sets next(m+1);

%Next, run the actual algorithm using the next table.
j=1;
k=1;

%Consider the special case of j=1 --skip forward to the first possible
%match.
while(array(k)~=pattern(1))
    k=k+1;
    if(k>n)%Nothing matches the first character of the pattern.
        startIdx=[];
        return;
    end
end

while(1)
    while((j<=m)&&(k<=n))
        while((j>0)&&(array(k)~=pattern(j)))
           j=next(j);
        end
        k=k+1;
        j=j+1;
    end

    if(j<=m)%All of the matches have been found.
        startIdx=startIdx(1:numFound);
        return;
    end

    %With j>m, the leftmost match has been found in positions k-m through
    %k-1.
    numFound=numFound+1;
    startIdx(numFound)=k-m;
    j=next(m+1);

    if(numFound==maxTimes)
        startIdx=startIdx(1:numFound);
        return;
    end
end

end

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


function check = weibull_discrete_check ( a, b )

%*****************************************************************************80
%
%% WEIBULL_DISCRETE_CHECK checks the parameters of the discrete Weibull CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 <= A <= 1.0,
%    0.0 < B.
%
%    Output, logical CHECK, is true if the parameters are legal.
%
  if ( a < 0.0 | 1.0 < a )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'WEIBULL_DISCRETE_CHECK - Fatal error!\n' );
    fprintf ( 1, '  A < 0 or 1 < A.\n' );
    check = 0;
    return
  end

  if ( b <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'WEIBULL_DISCRETE_CHECK - Fatal error!\n' );
    fprintf ( 1, '  B <= 0.\n' );
    check = 0;
    return
  end

  check = 1;

  return
end


function [sts] = ft_spiketriggeredspectrum_fft(cfg, data, spike)

% FT_SPIKETRIGGEREDSPECTRUM_FFT computes the Fourier spectrum (amplitude and phase)
% of the LFP around the % spikes.  A phase of zero corresponds to the spike being on
% the peak of the LFP oscillation. A phase of 180 degree corresponds to the spike being
% in the through of the oscillation. A phase of 45 degrees corresponds to the spike
% being just after the peak in the LFP.
%
% If the triggered spike leads a spike in another channel, then the angle of the Fourier
% spectrum of that other channel will be negative. Earlier phases are in clockwise
% direction.
%
% Use as
%   [sts] = ft_spiketriggeredspectrum_convol(cfg,data,spike)
% or
%   [sts] = ft_spiketriggeredspectrum_convol(cfg,data)
% where the spike data can either be contained in the DATA input or in the SPIKE input.
%
% The input DATA should be organised as the raw datatype, obtained from FT_PREPROCESSING
% or FT_APPENDSPIKE.
%
% The (optional) input SPIKE should be organised as the spike or the raw datatype,
% obtained from FT_SPIKE_MAKETRIALS or FT_PREPROCESSING (in that case, conversion is done
% within the function)
%
% Important is that data.time and spike.trialtime should be referenced relative to the
% same trial trigger times.
%
% The configuration should be according to
%   cfg.timwin       = [begin end], time around each spike (default = [-0.1 0.1])
%   cfg.foilim       = [begin end], frequency band of interest (default = [0 150])
%   cfg.taper        = 'dpss', 'hanning' or many others, see WINDOW (default = 'hanning')
%   cfg.tapsmofrq    = number, the amount of spectral smoothing through
%                      multi-tapering. Note that 4 Hz smoothing means plus-minus 4 Hz,
%                      i.e. a 8 Hz smoothing box. Note: multitapering rotates phases (no
%                      problem for consistency)
%   cfg.spikechannel = string, name of spike channels to trigger on cfg.channel = Nx1
%                      cell-array with selection of channels (default = 'all'),
%                      see FT_CHANNELSELECTION for details
%   cfg.feedback     = 'no', 'text', 'textbar', 'gui' (default = 'no')
%
% The output STS data structure can be input to FT_SPIKETRIGGEREDSPECTRUM_STAT
%
% This function uses a NaN-aware spectral estimation technique, which will default to the
% standard Matlab FFT routine if no NaNs are present. The fft_along_rows subfunction below
% demonstrates the expected function behavior.
%
% See FT_SPIKETRIGGEREDINTERPOLATION to remove segments of LFP around spikes.
% See FT_SPIKETRIGGEREDSPECTRUM_CONVOL for an alternative implementation based
% on convolution

% Copyright (C) 2008, Robert Oostenveld
%
% 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$

% these are used by the ft_preamble/ft_postamble function and scripts
ft_revision = '$Id$';
ft_nargin   = nargin;
ft_nargout  = nargout;

% do the general setup of the function
ft_defaults
ft_preamble init
ft_preamble provenance data spike

% check input data structure
data = ft_checkdata(data,'datatype', 'raw', 'feedback', 'yes');
if nargin==3
  spike = ft_checkdata(spike, 'datatype', {'spike'}, 'feedback', 'yes');
end

% these were supported in the past, but are not any more (for consistency with other spike functions)
cfg = ft_checkconfig(cfg, 'forbidden', {'inputfile','outputfile'});

%get the options
cfg.timwin       = ft_getopt(cfg, 'timwin',[-0.1 0.1]);
cfg.spikechannel = ft_getopt(cfg,'spikechannel', 'all');
cfg.channel      = ft_getopt(cfg,'channel', 'all');
cfg.feedback     = ft_getopt(cfg,'feedback', 'no');
cfg.tapsmofrq    = ft_getopt(cfg,'tapsmofrq', 4);
cfg.taper        = ft_getopt(cfg,'taper', 'hanning');
cfg.foilim       = ft_getopt(cfg,'foilim', [0 150]);

% ensure that the options are valid
cfg = ft_checkopt(cfg,'timwin','doublevector');
cfg = ft_checkopt(cfg,'spikechannel',{'cell', 'char', 'double', 'empty'});
cfg = ft_checkopt(cfg,'channel', {'cell', 'char', 'double'});
cfg = ft_checkopt(cfg,'feedback', 'char', {'yes', 'no'});
cfg = ft_checkopt(cfg,'taper', 'char');
cfg = ft_checkopt(cfg,'tapsmofrq', 'doublescalar');
cfg = ft_checkopt(cfg,'foilim', 'doublevector');

if strcmp(cfg.taper, 'sine')
  error('sorry, sine taper is not yet implemented');
end

% get the spikechannels
if nargin==2

  % autodetect the spikechannels and EEG channels
  [spikechannel, eegchannel] = detectspikechan(data);

  % make the final selection of spike channels and check
  if strcmp(cfg.spikechannel, 'all'),
    cfg.spikechannel = spikechannel;
  else
    cfg.spikechannel = ft_channelselection(cfg.spikechannel, data.label);
    if ~all(ismember(cfg.spikechannel,spikechannel)),
      error('some selected spike channels appear eeg channels');
    end
  end

  % make the final selection of EEG channels and check
  if strcmp(cfg.channel,'all')
    cfg.channel = eegchannel;
  else
    cfg.channel      = ft_channelselection(cfg.channel, data.label);
    if ~all(ismember(cfg.channel,eegchannel)),
      warning('some of the selected eeg channels appear spike channels');
    end
  end

  % select the data and convert to a spike structure
  tmpcfg = [];
  tmpcfg.channel = cfg.spikechannel;
  data_spk = ft_selectdata(tmpcfg, data);
  tmpcfg.channel = cfg.channel;
  data     = ft_selectdata(tmpcfg, data); % leave only LFP
  spike    = ft_checkdata(data_spk,'datatype', 'spike');
  clear data_spk % remove the continuous data
else
  cfg.spikechannel = ft_channelselection(cfg.spikechannel, spike.label);
  cfg.channel      = ft_channelselection(cfg.channel, data.label);
end

% determine the channel indices and number of chans
chansel          = match_str(data.label, cfg.channel); % selected channels
nchansel         = length(cfg.channel);                % number of channels
spikesel         = match_str(spike.label, cfg.spikechannel);
nspikesel        = length(spikesel); % number of spike channels
if nspikesel==0, error('no spike channel selected'); end

% construct the taper
if ~isfield(data, 'fsample'), data.fsample = 1/mean(diff(data.time{1})); end
begpad = round(cfg.timwin(1)*data.fsample);
endpad = round(cfg.timwin(2)*data.fsample);
numsmp = endpad - begpad + 1;
if ~strcmp(cfg.taper,'dpss')
  taper  = window(cfg.taper, numsmp);
  taper  = taper./norm(taper);
else
  % not implemented yet: keep tapers, or selecting only a subset of them.
  taper  = dpss(numsmp, cfg.tapsmofrq);
  taper  = taper(:,1:end-1);            % we get 2*NW-1 tapers
  taper  = sum(taper,2)./size(taper,2); % using the linearity of multitapering
end
taper  = sparse(diag(taper));

% preallocate the output structures for different units / trials
ntrial      = length(data.trial);
spectrum    = cell(nspikesel,ntrial);
spiketime   = cell(nspikesel,ntrial);
spiketrial  = cell(nspikesel,ntrial);

% select the frequencies
freqaxis = linspace(0, data.fsample, numsmp);
fbeg = nearest(freqaxis, cfg.foilim(1));
fend = nearest(freqaxis, cfg.foilim(2));

% update the configuration to account for rounding off differences
cfg.foilim(1) = freqaxis(fbeg);
cfg.foilim(2) = freqaxis(fend);

% make a representation of the spike, this is used for the phase rotation
spike_repr = zeros(1,numsmp);
time       = linspace(cfg.timwin(1),cfg.timwin(2), numsmp);
spike_repr(1-begpad) = 1;
spike_fft = specest_nanfft(spike_repr, time);
spike_fft = spike_fft(fbeg:fend);
spike_fft = spike_fft./abs(spike_fft);
rephase   = sparse(diag(conj(spike_fft)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute the spectra
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ft_progress('init', 'text',     'Please wait...');
for iUnit  = 1:nspikesel
  for iTrial = 1:ntrial

    % select the spikes that fell in the trial and convert to samples
    timeBins = [data.time{iTrial}  data.time{iTrial}(end)+1/data.fsample] - (0.5/data.fsample);
    hasTrial = spike.trial{spikesel(iUnit)} == iTrial; % find the spikes that are in the trial
    ts       = spike.time{spikesel(iUnit)}(hasTrial); % get the spike times for these spikes
    ts       = ts(ts>=timeBins(1) & ts<=timeBins(end)); % only select those spikes that fall in the trial window
    [ignore,spikesmp] = histc(ts,timeBins);
    if ~isempty(ts)
      ts(spikesmp==0 | spikesmp==length(timeBins)) = [];
    end

    spikesmp(spikesmp==0 | spikesmp==length(timeBins)) = [];

    % store in the output cell arrays as column vectors
    spiketime{iUnit, iTrial}  = ts(:);
    tr = iTrial*ones(size(spikesmp));
    spiketrial{iUnit, iTrial} = tr(:);

    % preallocate the spectrum
    spectrum{iUnit, iTrial} = zeros(length(spikesmp), nchansel, fend-fbeg+1);

    % compute the spiketriggered spectrum
    ft_progress(iTrial/ntrial, 'spectrally decomposing data for trial %d of %d, %d spikes for unit %d', iTrial, ntrial, length(spikesmp), iUnit);
    for j=1:length(spikesmp)

      % selected samples
      begsmp = spikesmp(j) + begpad;
      endsmp = spikesmp(j) + endpad;

      % handle spikes near the borders of the trials
      if (begsmp<1)
        segment = nan(nchansel, numsmp);
      elseif endsmp>size(data.trial{iTrial},2)
        segment = nan(nchansel, numsmp);
      else
        segment = data.trial{iTrial}(chansel,begsmp:endsmp);
      end

      % substract the DC component from every segment, to avoid any leakage of the taper
      segmentMean = repmat(nanmean(segment,2),1,numsmp); % nChan x Numsmp
      segment     = segment - segmentMean; % LFP has average of zero now (no DC)

      % taper the data segment around the spike and compute the fft
      segment_fft = specest_nanfft(segment * taper, time);

      % select the desired output frquencies and normalize
      segment_fft = segment_fft(:,fbeg:fend) ./ sqrt(numsmp/2);

      % rotate the estimated phase at each frequency to correct for the segment t=0 not being at the first sample
      segment_fft = segment_fft * rephase;

      % store the result for this spike in this trial
      spectrum{iUnit, iTrial}(j,:,:) = segment_fft;

    end % for each spike in this trial
  end % for each trial
end
ft_progress('close');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% collect the results in a structure that is a spike structure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sts.lfplabel       = data.label(chansel);
sts.freq           = freqaxis(fbeg:fend);
sts.dimord         = 'rpt_chan_freq';
for iUnit = 1:nspikesel
  sts.fourierspctrm{iUnit}  = cat(1, spectrum{iUnit,:});
  spectrum(iUnit,:) = {[]}; % free from the memory
  sts.time{iUnit}           = cat(1,spiketime{iUnit,:});
  sts.trial{iUnit}          = cat(1,spiketrial{iUnit,:});
end
sts.dimord    = '{chan}_spike_lfpchan_freq';
sts.trialtime = spike.trialtime;
sts.label     = spike.label(spikesel);

% do the general cleanup and bookkeeping at the end of the function

ft_postamble previous   data
ft_postamble provenance sts
ft_postamble history    sts

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [spikelabel, eeglabel] = detectspikechan(data)

maxRate = 1000; % default on what we still consider a neuronal signal

% autodetect the spike channels
ntrial = length(data.trial);
nchans  = length(data.label);
spikechan = zeros(nchans,1);
for i=1:ntrial
  for j=1:nchans
    hasAllInts    = all(isnan(data.trial{i}(j,:)) | data.trial{i}(j,:) == round(data.trial{i}(j,:)));
    hasAllPosInts = all(isnan(data.trial{i}(j,:)) | data.trial{i}(j,:)>=0);
    fr            = nansum(data.trial{i}(j,:),2) ./ (data.time{i}(end)-data.time{i}(1));
    spikechan(j) = spikechan(j) + double(hasAllInts & hasAllPosInts & fr<=maxRate);
  end
end
spikechan = (spikechan==ntrial);

spikelabel = data.label(spikechan);
eeglabel   = data.label(~spikechan);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION for demonstration purpose
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function y = fft_along_rows(x)
y = fft(x, [], 2); % use normal Matlab function to compute the fft along 2nd dimension


function vt = volToTalairach(volCoords, v2t)
% talairachCoords = volToTalairach(volCoords, v2t)
%
% PURPOSE:
%   Converts arbitrary coordinates (volCoords) to Talairach space, given
%   the transform info in the v2t struct. The coords must be in 'n X 3'
%   row-vector form. The v2t struct must have a 4x4 affine transform matrix
%
%       v2t.transRot
%
%   And 7 scale factors:
%
%       v2t.superiorAcScale
%       v2t.inferiorAcScale
%       v2t.rightAcScale
%       v2t.leftAcScale
%       v2t.anteriorAcScale
%       v2t.betweenAcPcScale
%       v2t.posteriorPcScale
%
%   See computeTalairach.m for an example of how to compute these.
%
% HISTORY:
%   2001.09.14 RFD (bob@white.stanford.edu) Wrote it, with help from
%              Alex Wade and Jochem Rieger.

% MrLoadRet coords are axial,coronal,sagittal, but we need coronal,axial,sagittal.
% Oh well...

% We have to make the coords homogeneous (ie. 4d)
vt = ones(size(volCoords,1),size(volCoords,2)+1);
vt(:,1:3) = [volCoords(:,2),volCoords(:,1),volCoords(:,3)];

% now, apply translation and rotation
vt = vt * v2t.transRot;

% v shoule now be on the talairach axes:
%   X = sagittal slice (right is +)
%   Y = coronal slice (anterior is +)
%   Z = axial slice (superior is +)
%
% We just need to apply the appropriate scale factor
for(ii=1:size(vt,1))
    if(vt(ii,1)>0)
        vt(ii,1) = vt(ii,1) * v2t.rightAcScale;
    else
        vt(ii,1) = vt(ii,1) * v2t.leftAcScale;
    end
    if(vt(ii,3)>0)
        vt(ii,3) = vt(ii,3) * v2t.superiorAcScale;
    else
        vt(ii,3) = vt(ii,3) * v2t.inferiorAcScale;
    end
     if(vt(ii,2)>0)
        vt(ii,2) = vt(ii,2) * v2t.anteriorAcScale;
    else
        acpc = vt(ii,2) * v2t.betweenAcPcScale;
        % we have to apply separate scale factors if it goes beyond the PC
        if(acpc<-24)
            beyondPC = vt(ii,2) + 24/v2t.betweenAcPcScale;
            vt(ii,2) = -24 + beyondPC*v2t.posteriorPcScale;
        else
            vt(ii,2) = acpc;
        end
    end
end
vt = vt(:,1:3);
return;


function [ ap, det, inert ] = zhpdi ( ap, n, ipvt, job )

%*****************************************************************************80
%
%% ZHPDI: determinant, inertia and inverse of a complex hermitian matrix.
%
%  Discussion:
%
%    The routine uses the factors from ZHPFA.
%
%    The matrix is stored in packed form.
%
%    A division by zero will occur if the inverse is requested and ZHPCO has
%    set RCOND == 0.0 or ZHPFA has set INFO ~= 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex AP(N*(N+1)/2); the factored matrix
%    from ZHPFA.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer IPVT(N), the pivot vector from ZHPFA.
%
%    Input, integer JOB, has the decimal expansion ABC where:
%    if C ~= 0, the inverse is computed,
%    if B ~= 0, the determinant is computed,
%    if A ~= 0, the inertia is computed.
%    For example, JOB = 111 gives all three.
%
%    Output, complex AP(N*(N+1)/2); if the inverse was requested, then
%    the upper triangle of the inverse of the original matrix, stored in packed
%    form.  The columns of the upper triangle are stored sequentially in a
%    one-dimensional array.
%
%    Output, real DET(2), if requested, the determinant of the original matrix.
%    Determinant = DET(1) * 10.0**DET(2) with 1.0 <= abs ( DET(1) ) < 10.0
%    or DET(1) = 0.0.
%
%    Output, integer INERT(3), if requested, the inertia of the original matrix.
%    INERT(1) = number of positive eigenvalues.
%    INERT(2) = number of negative eigenvalues.
%    INERT(3) = number of zero eigenvalues.
%
  det = [];
  inert = [];

  noinv = mod ( job, 10 ) == 0;
  nodet = floor ( mod ( job, 100 ) / 10 ) == 0;
  noert = floor ( mod ( job, 1000 ) / 100 ) == 0;

  if ( ~nodet | ~noert )

    if ( ~noert )
      inert(1:3) = 0;
    end

    if ( ~nodet )
      det(1) = 1.0;
      det(2) = 0.0;
    end

    t = 0.0;
    ik = 0;

    for k = 1 : n

      kk = ik + k;
      d = real ( ap(kk) );
%
%  Check if 1 by 1
%
      if ( ipvt(k) <= 0 )
%
%  2 by 2 block
%  Use DET (D  S; S  C)  =  ( D / T * C - T ) * T, T = abs ( S )
%  to avoid underflow/overflow troubles.
%  Take two passes through scaling.  Use T for flag.
%
        if ( t == 0.0 )
          ikp1 = ik + k;
          kkp1 = ikp1 + k;
          t = abs ( ap(kkp1) );
          d = ( d / t ) * real ( ap(kkp1+1) ) - t;
        else
          d = t;
          t = 0.0;
        end

      end

      if ( ~noert )

        if ( 0.0 < d )
          inert(1) = inert(1) + 1;
        elseif ( d < 0.0 )
          inert(2) = inert(2) + 1;
        elseif ( d == 0.0 )
          inert(3) = inert(3) + 1;
        end

      end

      if ( ~nodet )

        det(1) = det(1) * d;

        if ( det(1) ~= 0.0 )

          while ( abs ( det(1) ) < 1.0 )
            det(1) = det(1) * 10.0;
            det(2) = det(2) - 1.0;
          end

          while ( 10.0 <= abs ( det(1) ) )
            det(1) = det(1) / 10.0;
            det(2) = det(2) + 1.0;
          end

        end

      end

      ik = ik + k;

    end

  end
%
%  Compute inverse(A).
%
  if ( ~noinv )

    k = 1;
    ik = 0;

    while ( k <= n )

      km1 = k - 1;
      kk = ik + k;
      ikp1 = ik + k;
      kkp1 = ikp1 + k;
%
%  1 by 1
%
      if ( 0 <= ipvt(k) )

        ap(kk) = 1.0 / real ( ap(kk) );

        if ( 1 <= km1 )

          work(1:km1) = ap(ik+1:ik+km1);

          ij = 0;
          for j = 1 : km1
            jk = ik + j;
            ap(jk) = conj ( ap(ij+1:ij+j) ) * tranpose ( work(1:j) );
            ap(ik+1:ik+j-1) = ap(ik+1:ik+j-1) + work(j) * ap(ij+1:ij+j-1);
            ij = ij + j;
          end

          ap(kk) = ap(kk) + real ( conj ( work(1:km1) ) * transpose ( ap(ik+1:ik+km1) ) );

        end

        kstep = 1;
%
%  2 by 2
%
      else

        t = abs ( ap(kkp1) );
        ak = real ( ap(kk) ) / t;
        akp1 = real ( ap(kkp1+1) ) / t;
        akkp1 = ap(kkp1) / t;
        d = t * ( ak * akp1 - 1.0 );
        ap(kk) = akp1 / d;
        ap(kkp1+1) = ak / d;
        ap(kkp1) = -akkp1 / d;

        if ( 1 <= km1 )

          work(1:km1) = ap(ikp1+1:ikp1+km1);

          ij = 0;
          for j = 1 : km1
            jkp1 = ikp1 + j;
            ap(jkp1) = conj ( ap(ij+1:ij+j) ) * transpose ( work(1:j) );
            ap(ikp1+1:ikp1+j-1) = ap(ikp1+1:ikp1+j-1) ...
              + work(j) * ap(ij+1:ij+j-1);
            ij = ij + j;
          end

          ap(kkp1+1) = ap(kkp1+1) ...
            + real ( conj ( work(1:km1) ) * transpose ( ap(ikp1+1) ) );

          ap(kkp1) = ap(kkp1) ...
            + conj ( ap(ik+1:ik+km1) ) * transpose ( ap(ikp1+1:ikp1+km1) );

          work(1:km1) = ap(ik+1:ik+km1);

          ij = 0;

          for j = 1 : km1
            jk = ik + j;
            ap(jk) = conj ( ap(ij+1:ij+j) ) * transpose ( work(1:j) );
            ap(ik+1:ik+j-1) = ap(ik+1:ik+j-1) + work(j) * ap(ij+1:ij+j-1);
            ij = ij + j;
          end

          ap(kk) = ap(kk) ...
            + real ( conj ( work(1:km1) ) * transpose ( ap(ik+1:ik+km1) ) );

        end

        kstep = 2;

      end
%
%  Swap
%
      ks = abs ( ipvt(k) );

      if ( ks ~= k )

        iks = ( ks * ( ks - 1 ) ) / 2;

        temp             = ap(iks+1:iks+ks);
        ap(iks+1:iks+ks) = ap(ik+1:ik+ks);
        ap(ik+1:ik+ks)   = temp;

        ksj = ik + ks;

        for jb = ks : k

          j = k + ks - jb;
          jk = ik + j;

          temp    = conj ( ap(jk) );
          ap(jk)  = conj ( ap(ksj) );
          ap(ksj) = temp;

          ksj = ksj - ( j - 1 );

        end

        if ( kstep ~= 1 )

          kskp1 = ikp1 + ks;

          temp      = ap(kskp1);
          ap(kskp1) = ap(kkp1);
          ap(kkp1)  = temp;

        end

      end

      ik = ik + k;

      if ( kstep == 2 )
        ik = ik + k + 1;
      end

      k = k + kstep;

    end

  end

  return
end


% This demo loads two color images and efficiently computes the emd_hat
% between them. The ground distance is a thresholded linear combination of
% the spatial and color distance between pixels. This is similar to the
% distance I used for color images in my ICCV paper. Image sizes do not
% need to be the same. As a color distance I used CIEDE2000.
% emd_hat is described in the paper:
%  A Linear Time Histogram Metric for Improved SIFT Matching
%  Ofir Pele, Michael Werman
%  ECCV 2008
% The efficient algorithm is described in the paper:
%  Fast and Robust Earth Mover's Distances
%  Ofir Pele, Michael Werman
%  ICCV 2009
% CIEDE2000 is described in the papers:
% The development of the CIE 2000 colour-difference formula: CIEDE2000
%  M. R. Luo, G. Cui, B. Rigg
%  CRA 2001
% The CIEDE2000 color-difference formula: Implementation notes, supplementary test data, and mathematical observations
%  Gaurav Sharma, Wencheng Wu, Edul N. Dalal
%  CRA 2004

clc; close all; clear all;

addpath ../

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Distance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% In the paper I used threshold=20, but I didn't use alpha_color and
% 1-alpha_color for the spatial. I just added them. So using threshold=10
% is equivalent to what I used in the paper.
threshold= 10;
alpha_color= 1/2;
% Center. In the ICCV paper center and identity were equivalent as the image
% sizes were the same.
coordinates_transformation= 2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% load images
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
im1= imresize( imread('1.jpg') , 1/30);
im2= imresize( imread('2.jpg') , 1/30);
% 3.jpg is more similar to 1.jpg and thus the running time will be longer.
%im2= imresize( imread('3.jpg') , 1/30);
im1_Y= size(im1,1);
im1_X= size(im1,2);
im1_N= im1_Y*im1_X;
im2_Y= size(im2,1);
im2_X= size(im2,2);
im2_N= im2_Y*im2_X;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EMD input
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
P= [ ones(im1_N,1)  ;  zeros(im2_N,1) ];
Q= [ zeros(im1_N,1) ;  ones(im2_N,1)  ];

im1= double(im1)./255;
im2= double(im2)./255;
cform = makecform('srgb2lab');
im1_lab= applycform(im1, cform);
im2_lab= applycform(im2, cform);

% Creating the ground distance matrix.
% Loops in Matlab are very time consuming, so I use mex.
% Matlab corresponding code is commented out below.
tic
[ground_distance_matrix]= color_spatial_EMD_ground_distance(im1_lab,im2_lab,...
                                                  alpha_color,threshold, ...
                                                  coordinates_transformation);
fprintf(1,'computing the ground distance matrix, time in seconds: %f\n', ...
        toc);

% In ICCV paper extra_mass_penalty did not matter as image sizes were the same.
extra_mass_penalty= -1;
flowType= 3;

tic
[emd_hat_mex_val_with_flow F]= emd_hat_mex(P,Q,ground_distance_matrix,extra_mass_penalty,flowType);
fprintf(1,'Note that the ground distance here is not a metric.\n');
fprintf(1,'emd_hat_mex, time in seconds: %f\n',toc);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (c) 2009-2012, Ofir Pele
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%    * Redistributions of source code must retain the above copyright
%    notice, this list of conditions and the following disclaimer.
%    * Redistributions in binary form must reproduce the above copyright
%    notice, this list of conditions and the following disclaimer in the
%    documentation and/or other materials provided with the distribution.
%    * Neither the name of the The Hebrew University of Jerusalem nor the
%    names of its contributors may be used to endorse or promote products
%    derived from this software without specific prior written permission.

% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


function signal_mat = focus(kgrid, input_signal, source_mask, focus_position, sound_speed)
%FOCUS    Create input signal based on source mask and focus position.
%
% DESCRIPTION:
%       focus takes a single input signal and a source mask and creates an
%       input signal matrix (with one input signal for each source point).
%       The appropriate time delays required to focus the signals at a
%       given position in Cartesian space are automatically added based on
%       the user inputs for focus_position and sound_speed.
%
% USAGE:
%       input_signal_mat = focus(kgrid, input_signal, source_mask, focus_position, sound_speed)
%
% INPUTS:
%       kgrid           - k-Wave grid structure returned by makeGrid
%       input_signal    - single time series input
%       source_mask     - matrix specifying the positions of the time
%                         varying source distribution (i.e., source.p_mask
%                         or source.u_mask)
%       focus_position  - position of the focus in Cartesian coordinates
%       sound_speed     - scalar sound speed
%
% OUTPUTS:
%       input_signal_mat - matrix of time series following the source
%                         points using MATLAB's column-wise linear index
%                         ordering
%
% ABOUT:
%       author          - Bradley Treeby
%       date            - 21st February 2012
%       last update     - 25th July 2013
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave 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 Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% check that kgrid.t_array is defined
if strcmp(kgrid.t_array, 'auto')
    error('kgrid.t_array must be defined');
end

% check that the input sound speed is scalar
if numel(sound_speed) ~= 1
    error('sound_speed input must be scalar');
end

% calculate the distance from every point in the source mask to the focus
% position
switch kgrid.dim
    case 1
        dist = abs(kgrid.x(source_mask == 1) - focus_position(1));
    case 2
        dist = sqrt( (kgrid.x(source_mask == 1) - focus_position(1)).^2 + ...
            (kgrid.y(source_mask == 1) - focus_position(2)).^2 );
    case 3
        dist = sqrt( (kgrid.x(source_mask == 1) - focus_position(1)).^2 + ...
            (kgrid.y(source_mask == 1) - focus_position(2)).^2 + ...
            (kgrid.z(source_mask == 1) - focus_position(3)).^2 );
end

% convert the distance to units of time points
dist = round(dist./(kgrid.dt*sound_speed));

% convert time points to relative delays
dist = -(dist - max(dist(:)));
max_delay = max(dist(:));

% create an input matrix
signal_mat = zeros(length(dist), length(input_signal) + max_delay);

% assign the input signal
for source_index = 1:length(dist)
    delay = dist(source_index);
    signal_mat(source_index, :) = [zeros(1, delay), input_signal, zeros(1, max_delay - delay)];
end


function [M0,M1,L,X,q0] = spm_mfa(M,x,u)
% Jacobian for mean field approximations
% FORMAT [M0,M1,L,X,q0] = spm_mfa(M,x,u)
%--------------------------------------------------------------------------
% M   - model specification structure
% Required fields:
%    M.f   - dx/dt    = f(x,u,P)                     {function string or m-file}
%    M.g   - y(t)     = g(x,u,P)                     {function string or m-file}
%    M.m   - m inputs
%    M.n   - n states
%    M.l   - l ouputs
%    M.x   - (n x 1) = x(0) = expansion point
%    M.W   - (n x n) - covariance matrix of deterministic noise
% x    - cell array of vectors specifying evaluation grid
% u    - expansion point for inputs (c.f. background activity);
%
% M0   - 1st order Bilinear matrix dq/dt = M0*q + u*M1*q,  q = p(X);
% M1   - 2nd order Bilinear matrix
% L    - output matrix                 <y> = L*q;
% X    - evaluation points of state space
% q0   - stable mode M0*q0 = 0
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_mfa.m 4936 2012-09-18 19:47:55Z karl $

% default expansion point for inputs
%--------------------------------------------------------------------------
if nargin < 3
    u0 = zeros(M.m,1);
else
    u0 = u;
end

% create X - coordinates of evaluation grid
%--------------------------------------------------------------------------
n     = length(x);
for i = 1:n
    q     = 1;
    for j = 1:n
        q = kron(x{j}(:).^(i == j),q);
    end
    X(:,i) = q;
end

% f(x,0)
%--------------------------------------------------------------------------
N     = length(X);
f     = sparse(n,N);
for i = 1:N
    f(:,i) = feval(M.f,X(i,:)',u0,M.pE);
end

% Jacobian J - M0
%==========================================================================
for i = 1:n
    d(i)  = length(x{i});
    dx(i) = x{i}(2) - x{i}(1);
    I{i}  = speye(d(i),d(i));
end
J     = sparse(N,N);

% cycle over dimensions of state space
%--------------------------------------------------------------------------
for i = 1:length(x)

    % differential operators (for positive and negative flows)
    %----------------------------------------------------------------------
    j          = find(f(i,:) < 0);
    u          = sparse(j,1,f(i,j),N,1);
    j          = find(f(i,:) > 0);
    v          = sparse(j,1,f(i,j),N,1);

    dp         = spdiags(ones(d(i),1)*[1 -1],[ 0;1],d(i),d(i))/dx(i);
    dp(:,1)    = 0;
    dq         = spdiags(ones(d(i),1)*[1 -1],[-1;0],d(i),d(i))/dx(i);
    dq(:,end)  = 0;

    % Kronecker tensor products
    %----------------------------------------------------------------------
    Dp         = 1;
    Dq         = 1;
    for j = 1:(i - 1)
        Dp = kron(I{j},Dp);
        Dq = kron(I{j},Dq);
    end
    Dp    = kron(dp,Dp);
    Dq    = kron(dq,Dq);
    for j = (i + 1):n
        Dp = kron(I{j},Dp);
        Dq = kron(I{j},Dq);
    end
    DP{i} = Dp;

    % augment Jacobian
    %----------------------------------------------------------------------
    J     = J + Dp*spdiags(u,0,N,N) + Dq*spdiags(v,0,N,N);

end

% dispersion
%--------------------------------------------------------------------------
for i = 1:n
    for j = 1:n
        if M.W(i,j)
            J  = J - M.W(i,j)*DP{i}*DP{j}'/2;
        end

    end
end

% return if only M0 is required
%--------------------------------------------------------------------------
M0  = J;
if nargout == 1,  return,  end

% input induced changes to Jacobian dJ/du - M1
%==========================================================================
du    = 1e-6;
M1    = {};
for i = 1:M.m
    u     = u0;
    u(i)  = du;
    Mu    = spm_mfa(M,x,u);
    M1{i} = (Mu - M0)/du;
end

% return if only M0, M1 are required
%--------------------------------------------------------------------------
if nargout == 2,  return,  end

% Output matrix L - M1
%==========================================================================

% l(x,0)
%--------------------------------------------------------------------------
L     = sparse(M.l,N);
for i = 1:N
    L(:,i) = feval(M.g,X(i,:)',u,M.pE);
end

% equilibrium mode
%--------------------------------------------------------------------------
if nargout == 5

    % stable mode - stochastic iteration
    %----------------------------------------------------------------------
    q   = ones(N,1)/N;
    for i = 1:1024
        dq = M0*q;
        q  = q + dq/N;
        q  = q/sum(q);
    end
    q0  = q;

    % analytic solution (not implemented)
    %----------------------------------------------------------------------
    % q   = length(M0);
    % J   = [M0; ones(1,q)];
    % q0  = sum(inv(J'*J),2);

end


function [irf_record, D_record, gamma_record, struct_irf_record, irf_estimates, D_estimates, gamma_estimates, strshocks_record, strshocks_estimates]...
    =panel2irf(Ymat,Xmat,beta_gibbs,sigma_gibbs,It,Bu,IRFperiods,IRFband,N,n,m,p,k,T,Y,X,signreslabels,FEVDresperiods,data_exo,const,exo,IRFt,strctident,favar,signrestable,signresperiods)

% because there is only one model, there is no need to loop over units
% hence, estimate the IRFs for the model, as with a standard normal-Wishart
% first run the Gibbs sampler to obtain posterior draws
[irf_record]=bear.irf(beta_gibbs,It,Bu,IRFperiods,n,m,p,k);

% If IRFs have been set to an unrestricted VAR (IRFt=1):
if IRFt==1
% run a pseudo Gibbs sampler to obtain records for D and gamma (for the trivial SVAR)
[D_record, gamma_record]=bear.irfunres(n,It,Bu,sigma_gibbs);
struct_irf_record=[];
% compute posterior estimates
[irf_estimates,D_estimates,gamma_estimates]=bear.irfestimates(irf_record,n,IRFperiods,IRFband,IRFt,[],[],favar);

% If IRFs have been set to an SVAR with Choleski identification (IRFt=2):
elseif IRFt==2
% run the Gibbs sampler to transform unrestricted draws into orthogonalised draws
[struct_irf_record, D_record, gamma_record]=bear.irfchol(sigma_gibbs,irf_record,It,Bu,IRFperiods,n,favar);
% compute posterior estimates
[irf_estimates,D_estimates,gamma_estimates]=bear.irfestimates(struct_irf_record,n,IRFperiods,IRFband,IRFt,D_record,gamma_record,favar);

% If IRFs have been set to an SVAR with triangular factorisation (IRFt=3):
elseif IRFt==3
% run the Gibbs sampler to transform unrestricted draws into orthogonalised draws
[struct_irf_record, D_record, gamma_record]=bear.irftrig(sigma_gibbs,irf_record,It,Bu,IRFperiods,n,favar);
% compute posterior estimates
[irf_estimates,D_estimates,gamma_estimates]=bear.irfestimates(struct_irf_record,n,IRFperiods,IRFband,IRFt,D_record,gamma_record,favar);

% If IRFs have been set to an SVAR with sign restrictions (IRFt=4):
elseif IRFt==4
% run the Gibbs sampler to transform unrestricted draws into orthogonalised draws
[struct_irf_record,D_record,gamma_record]=bear.irfres_old(beta_gibbs,sigma_gibbs,It,Bu,IRFperiods,n,m,p,k,signrestable,signresperiods);
%[struct_irf_record,D_record,gamma_record]=bear.irfsignrespanel(beta_gibbs,sigma_gibbs,It,Bu,IRFperiods,n,p,m,k,signrestable,signresperiods);
%[struct_irf_record,D_record,gamma_record,~,~,~,~]=bear.irfres(beta_gibbs,sigma_gibbs,[],[],IRFperiods,n,m,p,k,T,[],[],signreslabels,FEVDresperiods,data_exo,HD,const,exo,strctident,pref,favar,IRFt,It,Bu);

% compute posterior estimates
[irf_estimates,D_estimates,gamma_estimates]=bear.irfestimates(struct_irf_record,n,IRFperiods,IRFband,IRFt,D_record,gamma_record,favar);

end

% also, if a s structural identification was implemented, compute structural shocks
strshocks_record={};
strshocks_estimates={};
if IRFt~=1
   % because shocks have to be computed for each unit, loop over units
   for ii=1:N
   % run the Gibbs sampler
   strshocks_record(:,:,ii)=bear.strshocks(beta_gibbs,D_record,Ymat(:,:,ii),Xmat(:,:,ii),n,k,It,Bu,favar);
   % obtain point estimates and credibility intervals
   strshocks_estimates(:,:,ii)=bear.strsestimates(strshocks_record(:,:,ii),n,T,IRFband);
   end
end


function f_x = ParFor5(in1)
%PARFOR5
%    F_X = PARFOR5(IN1)

%    This function was generated by the Symbolic Math Toolbox version 8.1.
%    16-Jul-2019 11:57:25

u = in1(:,1);
uxx = in1(:,5);
f_x = (u.*9.981631376631412e-1+uxx.*9.965561702440027+u.*uxx.*1.006065455272619)./(u+9.978905414318433);


function [ Sim QTot ] = computeAnimSimilarity( anim, method, ...
  varargin )
% Compute similarities between frames of a video sequence
%
% This routine computes:
%  - the pair infimum of CSFM if method = -2
%  - the best 2-view reconstruction in MSFM if method = 2
%  - the best 3-view reconstruction in MSFM if method = 3
%
% USAGE
%  [ Sim QTot ] = computeAnimSimilarity( anim, method, Sim )
%
% INPUTS
%  anim     - anim object (see GENERATETOYANIMATION for details)
%  method   - [2] uses pairs of frames, 3, uses triplets
%  varargin   - list of paramaters in quotes alternating with their values
%    for triadic comparison
%       'Sim2' pairwise similarity computed using method = 2
%       'perc' percentage of triplets to consider
%             ( only for speed purposes )
%
% OUTPUTS
%  Sim      - [ nFrame x nFrame ] or [ nFrame x nFrame x nFrame ]similarity
%             matrix based on total reconstruction error
%  QTot     - [ 4 x nFrame x nFrame ] optimal quaternions for pairwise
%
% EXAMPLE
%
% See also GENERATETOYANIMATION, VIEWANIMSIMILARITY
%
% Vincent's Structure From Motion Toolbox      Version 3.1.1
% Copyright (C) 2008-2011 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

if ~isa(anim,'Animation'); error('anim must be of class Animation'); end
nFrame=anim.nFrame;

if nargin<2; method=2; end

% Compute the similarities
switch abs( method )
  case 2
    anim = anim.centerW();

    [ Sim, QTot ] = computeCsfmInfimum(anim.W);

    if method==2 % Best 2-view reconstruction for MSFM
      Sim = Sim/2;
    end

  case 3
    %%%%%%%% using triplets of frames for MSFM
    [ Sim2 perc ] = getPrmDflt( varargin, { 'Sim2' [] 'perc' 1 } );
    Sim = cell(1,nFrame); for i=1:nFrame; Sim{i}=sparse(nFrame,nFrame); end
    isDone = cell(1,nFrame); for i=1:nFrame; isDone{i}=sparse(nFrame,nFrame); end
    minSpan = 5;

    ticId = ticStatus( 'Triadic Computed' );

    % compute the probability to choose another frame given a frame in the triplet
    % basically, we'll pick one frame, and the 2 other frames according to that
    % probability. This one simply is proportional to the pairwise reconstruction
    % error (3 frames are more likely to be together if, pairwise, they look alike)
    probS = Sim2;

    % remove everything too close to each other
    toep = logical(spdiags(ones(nFrame,2*minSpan+1,-minSpan:minSpan, nFrame,nFrame)));
    probS(toep) = 0;

    % compute the cumulative sum and normalize by one
    probS = cumsum(Sim2,2);
    probS = bsxfun(@rdivide,probS,probS(:,end));

    % create as many triplets as necessary
    nDone = 0;
    while nDone/nFrame^3*100 < perc
      % choose two other frames according to the distribution
      % in probS and far enough from each other
      sample = randSample(nFrame,3);
      i1 = sample(1); i2 = sample(2); i3 = sample(3);
      if isDone{i1}(i2,i3) || min( pdist( sample' ) )<=minSpan; continue; end

      [ disc disc err ] = computeSMFromW( anim.isProj, ...
        anim.W(:,:, [ i1 i2 i3]), 'K', anim.K, 'method',Inf);

      perm = [ i1 i2 i3; i1 i3 i2; i3 i1 i2; i3 i2 i1; i2 i1 i3; ...
        i2 i3 i1 ];
      for l=1:6
        Sim{perm(l,1)}(perm(l,2),perm(l,3)) = err(1);
        isDone{perm(l,1)}(perm(l,2),perm(l,3)) = 1;
        nDone = nDone + 1;
      end
      if rand()>0.99
        tocStatus(ticId,min(perc,100*nDone/nFrame^3) / perc);
      end
    end
end


function net = gpunpak(net, hp)
%GPUNPAK Separates hyperparameter vector into components.
%
%	Description
%	NET = GPUNPAK(NET, HP) takes an Gaussian Process data structure NET
%	and  a hyperparameter vector HP, and returns a Gaussian Process data
%	structure  identical to the input model, except that the covariance
%	bias BIAS, output noise NOISE, the input weight vector INWEIGHTS and
%	the vector of covariance function specific parameters  FPAR have all
%	been set to the corresponding elements of HP.
%
%	See also
%	GP, GPPAK, GPFWD, GPERR, GPGRAD
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Check arguments for consistency
errstring = consist(net, 'gp');
if ~isempty(errstring);
  error(errstring);
end
if net.nwts ~= length(hp)
  error('Invalid weight vector length');
end

net.bias = hp(1);
net.noise = hp(2);

% Unpack input weights
mark1 = 2 + net.nin;
net.inweights = hp(3:mark1);

% Unpack function specific parameters
net.fpar = hp(mark1 + 1:size(hp, 2));


function T = isotopes(varargin)
%ISOTOPES Returns NMR properties of the isotopes
% T = isotopes(i1, i2, ...)
%
% EXAMPLE
%     H = isotopes('1H'), T = isotopes('3H'), or if the atomic mass is not
%     specified, i.e. I = isotopes('H'), the function returns array of
%     structures with data for all isotopes of the specified element.
%     isotopes('n') returns NMR properties for neutron. Isotopes called
%     without arguments returns the whole table.
%
% The output structure has the following fields:
%     .name - element name;
%     .isotope - element symbol and mass number, i.e. 19F;
%     .Z - atomic number;
%     .mass - mass number;
%     .I - nuclear spin;
%     .gamma - magnetogyric ratio in rad/(T s) calculated from the resonant
%              frequency: 2*pi*F(@1T);
%     .ResFreq - resonant frequency in Hertz for an applied field B0 of 1 tesla
%                (in cgs units, 10 kilogauss);
%     .RelSensB - sensitivity relative to 1H (=1) assuming an equal number
%                 of nuclei, constant temperature, and constant B0:
%                 0.0076508(mu/mu_N)^3(I + 1)/I^2;
%     .RelSensF - sensitivity relative to 1H (=1) assuming an equal number
%                 of nuclei, constant temperature, and constant resonance
%                 frequency: O.23871(mu/mu_N)(I + 1);
%     .u - nuclear magnetic moment in units of the nuclear magneton, (mu/mu_N);
%     .Q - nuclear quadrupole moment in units of femtometers squared
%          (1 fm2 = 10^-2 barn)
%
%
% REFERENCES
% 1. "Nuclear spins, moments, and other data related to NMR spectroscopy" (9-93)
%    CRC Handbook of Chemistry and Physics, 83th Ed., CRC Press, Boca Raton, FL,
%    2002.
% 2. Stone, N. J., <www.nndc.bnl.gov/nndc/stone_moments/>
% 3. http://www.hbcpnetbase.com/

% Copyright (c) 2002 by Peter L. Volegov (volegov@unm.edu)
% All Rights Reserved.

%% Table of the Isotopes
% Element, Name, Z, AtomicMass, Spin, F@1T/MHz, RelSens(H), RelSens(F), mu/mu_N, Q/fm2
TheTable = {
'n', 'neutron', 1, 1, 1/2, 29.1647, 0.32139, 0.6850, -1.913042720, 0.0000; ...
'H', 'Hydrogen', 1, 1, 1/2, 42.5775, 1.00000, 1.0000, 2.792847337, 0.0000; ...
'H', 'Deuterium', 1, 2, 1, 6.5359, 0.00965, 0.4093, 0.857438228, 0.2860; ...
'H', 'Tritium', 1, 3, 1/2, 45.4148, 1.21354, 1.0666, 2.978962500, 0.0000; ...
'He', 'Helium', 2, 3, 1/2, 32.4360, 0.44212, 0.7618, -2.127624800, 0.0000; ...
'Li', 'Lithium', 3, 6, 1, 6.2661, 0.00850, 0.3925, 0.822046700, -0.0808; ...
'Li', 'Lithium', 3, 7, 3/2, 16.5483, 0.29356, 1.9433, 3.256440000, -4.0100; ...
'Be', 'Beryllium', 4, 9, 3/2, 5.9842, 0.01388, 0.7027, -1.177600000, 5.2880; ...
'B', 'Boron', 5, 10, 3, 4.5752, 0.01985, 1.7193, 1.800645000, 8.4590; ...
'B', 'Boron', 5, 11, 3/2, 13.6630, 0.16522, 1.6045, 2.688649000, 4.0590; ...
'C', 'Carbon', 6, 13, 1/2, 10.7084, 0.01591, 0.2515, 0.702411800, 0.0000; ...
'N', 'Nitrogen', 7, 14, 1, 3.0777, 0.00101, 0.1928, 0.403761000, 2.0440; ...
'N', 'Nitrogen', 7, 15, 1/2, 4.3173, 0.00104, 0.1014, -0.283188800, 0.0000; ...
'O', 'Oxygen', 8, 17, 5/2, 5.7742, 0.02910, 1.5822, -1.893790000, -2.5580; ...
'F', 'Fluorine', 9, 19, 1/2, 40.0776, 0.83400, 0.9413, 2.628868000, 0.0000; ...
'Ne', 'Neon', 10, 21, 3/2, 3.3631, 0.00246, 0.3949, -0.661797000, 10.1550; ...
'Na', 'Sodium', 11, 23, 3/2, 11.2688, 0.09270, 1.3233, 2.217522000, 10.4000; ...
'Mg', 'Magnesium', 12, 25, 5/2, 2.6083, 0.00268, 0.7147, -0.855450000, 19.9400; ...
'Al', 'Aluminum', 13, 27, 5/2, 11.1031, 0.20689, 3.0424, 3.641507000, 14.6600; ...
'Si', 'Silicon', 14, 29, 1/2, 8.4655, 0.00786, 0.1988, -0.555290000, 0.0000; ...
'P', 'Phosphorus', 15, 31, 1/2, 17.2515, 0.06652, 0.4052, 1.131600000, 0.0000; ...
'S', 'Sulfur', 16, 33, 3/2, 3.2717, 0.00227, 0.3842, 0.643821200, -6.7800; ...
'Cl', 'Chlorine', 17, 35, 3/2, 4.1765, 0.00472, 0.4905, 0.821874300, -8.1650; ...
'Cl', 'Chlorine', 17, 37, 3/2, 3.4765, 0.00272, 0.4083, 0.684123600, -6.4350; ...
'Ar', 'Argone', 18, 37, 3/2, 5.8190, 0.01276, 0.6833, 1.145000000, 0.0000; ...
'Ar', 'Argone', 18, 39, 7/2, 3.4600, 0.01130, 1.7079, -1.590000000, 0.0000; ...
'K', 'Potassium', 19, 39, 3/2, 1.9893, 0.00051, 0.2336, 0.391466200, 5.8500; ...
'K', 'Potassium', 19, 40, 4, 2.4737, 0.00523, 1.5493, -1.298100000, -7.3000; ...
'K', 'Potassium', 19, 41, 3/2, 1.0919, 8.00000, 0.1282, 0.214870100, 7.1100; ...
'Ca', 'Calcium', 20, 43, 7/2, 2.8688, 0.00642, 1.4150, -1.317260000, -4.0800; ...
'Sc', 'Scandium', 21, 45, 7/2, 10.3591, 0.30244, 5.1093, 4.756487000, -22.0000; ...
'Ti', 'Titanium', 22, 47, 5/2, 2.4041, 0.00210, 0.6587, -0.788480000, 30.2000; ...
'Ti', 'Titanium', 22, 49, 7/2, 2.4048, 0.00378, 1.1861, -1.104170000, 24.7000; ...
'V', 'Vanadium', 23, 50, 6, 4.2505, 0.05571, 5.5904, 3.345689000, 21.0000; ...
'V', 'Vanadium', 23, 51, 7/2, 11.2133, 0.38360, 5.5306, 5.148705700, -5.2000; ...
'Cr', 'Chromium', 24, 53, 3/2, 2.4115, 0.00091, 0.2832, -0.474540000, -15.0000; ...
'Mn', 'Manganese', 25, 55, 5/2, 10.5763, 0.17881, 2.8980, 3.468720000, 33.0000; ...
'Fe', 'Iron', 26, 57, 1/2, 1.3816, 0.00003, 0.0324, 0.090623000, 0.0000; ...
'Co', 'Cobalt', 27, 59, 7/2, 10.0770, 0.27841, 4.9702, 4.627000000, 42.0000; ...
'Ni', 'Nickel', 28, 61, 3/2, 3.8114, 0.00359, 0.4476, -0.750020000, 16.2000; ...
'Cu', 'Copper', 29, 63, 3/2, 11.2982, 0.09342, 1.3268, 2.223290000, -22.0000; ...
'Cu', 'Copper', 29, 65, 3/2, 12.1030, 0.11484, 1.4213, 2.381670000, -20.4000; ...
'Zn', 'Zinc', 30, 67, 5/2, 2.6694, 0.00287, 0.7314, 0.875479000, 15.0000; ...
'Ga', 'Gallium', 31, 69, 3/2, 10.2478, 0.06971, 1.2034, 2.016590000, 17.1000; ...
'Ga', 'Gallium', 31, 71, 3/2, 13.0208, 0.14300, 1.5291, 2.562270000, 10.7000; ...
'Ge', 'Germanium', 32, 73, 9/2, 1.4897, 0.00141, 1.1546, -0.879467700, -19.6000; ...
'As', 'Arsenic', 33, 75, 3/2, 7.3150, 0.02536, 0.8590, 1.439475000, 31.4000; ...
'Se', 'Selenium', 34, 77, 1/2, 8.1571, 0.00703, 0.1916, 0.535060000, 0.0000; ...
'Br', 'Bromine', 35, 79, 3/2, 10.7042, 0.07945, 1.2570, 2.106400000, 31.3000; ...
'Br', 'Bromine', 35, 81, 3/2, 11.5384, 0.09951, 1.3550, 2.270562000, 26.2000; ...
'Kr', 'Krypton', 36, 83, 9/2, 1.6442, 0.00190, 1.2744, -0.970669000, 25.9000; ...
'Rb', 'Rubidium', 37, 85, 5/2, 4.1254, 0.01061, 1.1304, 1.353030000, 27.6000; ...
'Rb', 'Rubidium', 37, 87, 3/2, 13.9811, 0.17703, 1.6418, 2.751240000, 13.3500; ...
'Sr', 'Strontium', 38, 87, 9/2, 1.8525, 0.00272, 1.4358, -1.093603000, 33.5000; ...
'Y', 'Yttrium', 39, 89, 1/2, 2.0949, 0.00012, 0.0492, -0.137415400, 0.0000; ...
'Zr', 'Zirconium', 40, 91, 5/2, 3.9748, 0.00949, 1.0891, -1.303620000, -17.6000; ...
'Nb', 'Niobium', 41, 93, 9/2, 10.4523, 0.48821, 8.1011, 6.170500000, -32.0000; ...
'Mo', 'Molybdenum', 42, 95, 5/2, 2.7874, 0.00327, 0.7638, -0.914200000, -2.2000; ...
'Mo', 'Molybdenum', 42, 97, 5/2, 2.8463, 0.00349, 0.7799, -0.933500000, 25.5000; ...
'Tc', 'Technetium', 43, 99, 9/2, 9.6294, 0.38174, 7.4633, 5.684700000, -12.9000; ...
'Ru', 'Ruthenium', 44, 99, 5/2, 1.9553, 0.00113, 0.5358, -0.641300000, 7.9000; ...
'Ru', 'Ruthenium', 44, 101, 5/2, 2.1916, 0.00159, 0.6005, -0.718800000, 45.7000; ...
'Rh', 'Rhodium', 45, 103, 1/2, 1.3477, 0.00003, 0.0317, -0.088400000, 0.0000; ...
'Pd', 'Palladium', 46, 105, 5/2, 1.9570, 0.00113, 0.5364, -0.642000000, 66.0000; ...
'Ag', 'Silver', 47, 107, 1/2, 1.7331, 0.00007, 0.0407, -0.113679600, 0.0000; ...
'Ag', 'Silver', 47, 109, 1/2, 1.9924, 0.00010, 0.0468, -0.130690600, 0.0000; ...
'Cd', 'Cadmium', 48, 111, 1/2, 9.0692, 0.00966, 0.2130, -0.594886100, 0.0000; ...
'Cd', 'Cadmium', 48, 113, 1/2, 9.4871, 0.01106, 0.2228, -0.622300900, 0.0000; ...
'In', 'Indium', 49, 113, 9/2, 9.3655, 0.35121, 7.2588, 5.528900000, 79.9000; ...
'In', 'Indium', 49, 115, 9/2, 9.3856, 0.35348, 7.2744, 5.540800000, 81.0000; ...
'Sn', 'Tin', 50, 115, 1/2, 14.0077, 0.03561, 0.3290, -0.918830000, 0.0000; ...
'Sn', 'Tin', 50, 117, 1/2, 15.2610, 0.04605, 0.3584, -1.001040000, 0.0000; ...
'Sn', 'Tin', 50, 119, 1/2, 15.9660, 0.05273, 0.3750, -1.047280000, 0.0000; ...
'Sb', 'Antimony', 51, 121, 5/2, 10.2551, 0.16302, 2.8100, 3.363400000, -36.0000; ...
'Sb', 'Antimony', 51, 123, 7/2, 5.5532, 0.04659, 2.7389, 2.549800000, -49.0000; ...
'Te', 'Tellurium', 52, 123, 1/2, 11.2349, 0.01837, 0.2639, -0.736947800, 0.0000; ...
'Te', 'Tellurium', 52, 125, 1/2, 13.5454, 0.03220, 0.3181, -0.888505100, 0.0000; ...
'I', 'Iodine', 53, 127, 5/2, 8.5778, 0.09540, 2.3504, 2.813273000, -71.0000; ...
'Xe', 'Xenon', 54, 129, 1/2, 11.8604, 0.02162, 0.2786, -0.777976300, 0.0000; ...
'Xe', 'Xenon', 54, 131, 3/2, 3.5159, 0.00282, 0.4129, 0.691861900, -11.4000; ...
'Cs', 'Cesium', 55, 133, 7/2, 5.6234, 0.04838, 2.7735, 2.582025000, -0.3430; ...
'Ba', 'Barium', 56, 135, 3/2, 4.2582, 0.00500, 0.5001, 0.837943000, 16.0000; ...
'Ba', 'Barium', 56, 137, 3/2, 4.7634, 0.00700, 0.5594, 0.937365000, 24.5000; ...
'La', 'Lanthanum', 57, 138, 5, 5.6615, 0.09404, 5.3188, 3.713646000, 45.0000; ...
'La', 'Lanthanum', 57, 139, 7/2, 6.0612, 0.06058, 2.9895, 2.783045500, 20.0000; ...
'Ce', 'Cerium', 58, 137, 3/2, 4.8800, 0.00752, 0.5729, 0.960000000, 0.0000; ...
'Ce', 'Cerium', 58, 139, 3/2, 5.3900, 0.01012, 0.6326, 1.060000000, 0.0000; ...
'Ce', 'Cerium', 58, 141, 7/2, 2.3700, 0.00364, 1.1708, 1.090000000, 0.0000; ...
'Pr', 'Praseodymium', 59, 141, 5/2, 13.0359, 0.33483, 3.5720, 4.275400000, -5.9000; ...
'Nd', 'Neodymium', 60, 143, 7/2, 2.3190, 0.00339, 1.1440, -1.065000000, -63.0000; ...
'Nd', 'Neodymium', 60, 145, 7/2, 1.4290, 0.00079, 0.7047, -0.656000000, -33.0000; ...
'Pm', 'Promethium', 61, 143, 5/2, 11.5900, 0.23510, 3.1748, 3.800000000, 0.0000; ...
'Pm', 'Promethium', 61, 147, 7/2, 5.6200, 0.04827, 2.7714, 2.580000000, 74.0000; ...
'Sm', 'Samarium', 62, 147, 7/2, 1.7748, 0.00152, 0.8753, -0.814900000, -26.0000; ...
'Sm', 'Samarium', 62, 149, 7/2, 14631.0000, 0.00085, 0.7216, -0.671800000, 7.4000; ...
'Eu', 'Europium', 63, 151, 5/2, 10.5856, 0.17929, 2.9006, 3.471800000, 90.3000; ...
'Eu', 'Europium', 63, 153, 5/2, 4.6745, 0.01544, 1.2809, 1.533100000, 241.0000; ...
'Gd', 'Gadolinium', 64, 155, 3/2, 1.3120, 0.00015, 0.1541, -0.258200000, 127.0000; ...
'Gd', 'Gadolinium', 64, 157, 3/2, 1.7200, 0.00033, 0.2020, -0.338500000, 135.0000; ...
'Tb', 'Terbium', 65, 159, 3/2, 10.2300, 0.06945, 1.2019, 2.014000000, 143.2000; ...
'Dy', 'Dysprosium', 66, 161, 5/2, 1.4654, 0.00048, 0.4015, -0.480600000, 250.7000; ...
'Dy', 'Dysprosium', 66, 163, 5/2, 2.0508, 0.00130, 0.5619, 0.672600000, 265.0000; ...
'Ho', 'Holmium', 67, 165, 7/2, 9.0883, 0.20423, 4.4825, 4.173000000, 358.0000; ...
'Er', 'Erbium', 68, 167, 7/2, 1.2281, 0.00050, 0.6057, -0.563900000, 356.5000; ...
'Tm', 'Thulium', 69, 169, 1/2, 3.5310, 0.00057, 0.0829, -0.231600000, 0.0000; ...
'Yb', 'Ytterbium', 70, 171, 1/2, 7.5261, 0.00552, 0.1768, 0.493670000, 0.0000; ...
'Yb', 'Ytterbium', 70, 173, 5/2, 2.0730, 0.00135, 0.5680, -0.679890000, 280.0000; ...
'Lu', 'Lutetium', 71, 175, 7/2, 4.8626, 0.03128, 2.3983, 2.232700000, 349.0000; ...
'Lu', 'Lutetium', 71, 176, 7, 3.4510, 0.03975, 6.0516, 3.169000000, 497.0000; ...
'Hf', 'Hafnium', 72, 177, 7/2, 1.7282, 0.00140, 0.8524, 0.793500000, 336.5000; ...
'Hf', 'Hafnium', 72, 179, 9/2, 1.0856, 0.00055, 0.8414, -0.640900000, 379.3000; ...
'Ta', 'Tantalum', 73, 180, 9, 4.0870, 0.10610, 11.5175, 4.825000000, 0.0000; ...
'Ta', 'Tantalum', 73, 181, 7/2, 5.1627, 0.03744, 2.5463, 2.370500000, 317.0000; ...
'W', 'Tungsten', 74, 183, 1/2, 1.7957, 0.00008, 0.0422, 0.117784800, 0.0000; ...
'Re', 'Rhenium', 75, 185, 5/2, 9.7176, 0.13870, 2.6627, 3.187100000, 218.0000; ...
'Re', 'Rhenium', 75, 187, 5/2, 9.8170, 0.14300, 2.6900, 3.219700000, 207.0000; ...
'Os', 'Osmium', 76, 187, 1/2, 0.9856, 0.00001, 0.0231, 0.064651890, 0.0000; ...
'Os', 'Osmium', 76, 189, 3/2, 3.3536, 0.00244, 0.3938, 0.659933000, 85.6000; ...
'Ir', 'Iridium', 77, 191, 3/2, 0.7658, 0.00003, 0.0899, 0.150700000, 81.6000; ...
'Ir', 'Iridium', 77, 191, 3/2, 0.8319, 0.00004, 0.0977, 0.163700000, 75.1000; ...
'Pt', 'Platinum', 78, 195, 1/2, 9.2922, 0.01039, 0.2182, 0.609520000, 0.0000; ...
'Au', 'Gold', 79, 197, 3/2, 0.7406, 0.00003, 0.0870, 0.145746000, 54.7000; ...
'Hg', 'Mercury', 80, 199, 1/2, 7.7123, 0.00594, 0.1811, 0.505885500, 0.0000; ...
'Hg', 'Mercury', 80, 201, 3/2, 2.8469, 0.00149, 0.3343, -0.560225700, 38.6000; ...
'Tl', 'Thallium', 81, 203, 1/2, 24.7316, 0.19598, 0.5809, 1.622257900, 0.0000; ...
'Tl', 'Thallium', 81, 205, 1/2, 24.9749, 0.20182, 0.5866, 1.638214600, 0.0000; ...
'Pb', 'Lead', 82, 207, 1/2, 9.0340, 0.00955, 0.2122, 0.592580000, 0.0000; ...
'Bi', 'Bismuth', 83, 209, 9/2, 6.9630, 0.14433, 5.3967, 4.110600000, 51.6000; ...
'Po', 'Polonium', 84, 209, 1/2, 11.7000, 0.02096, 0.2757, 0.770000000, 0.0000; ...
'Rn', 'Radon', 86, 211, 1/2, 9.1600, 0.00997, 0.2152, 0.601000000, 0.0000; ...
'Fr', 'Francium', 87, 223, 3/2, 5.9500, 0.01362, 0.6982, 1.170000000, 117.0000; ...
'Ra', 'Radium', 88, 223, 3/2, 1.3746, 0.00017, 0.1614, 0.270500000, 125.0000; ...
'Ra', 'Radium', 88, 225, 1/2, 11.1870, 0.01814, 0.2627, -0.733800000, 0.0000; ...
'Ac', 'Actinium', 89, 227, 3/2, 5.6000, 0.01131, 0.6564, 1.100000000, 170.0000; ...
'Th', 'Thorium', 90, 229, 5/2, 1.4000, 0.00042, 0.3843, 0.460000000, 430.0000; ...
'Pa', 'Protactinium', 91, 231, 3/2, 10.2000, 0.06903, 1.1995, 2.010000000, -172.0000; ...
'U', 'Uranium', 92, 235, 7/2, 0.8300, 0.00015, 0.4082, -0.380000000, 493.6000; ...
'Np', 'Neptunium', 93, 237, 5/2, 9.5700, 0.13264, 2.6234, 3.140000000, 388.6000; ...
'Pu', 'Plutonium', 94, 239, 1/2, 3.0900, 0.00038, 0.0727, 0.203000000, 0.0000; ...
'Am', 'Americium', 95, 243, 5/2, 4.6000, 0.01446, 1.2532, 1.500000000, 421.0000; ...
};

%% Find isotopes
if nargin == 0
    % Return the whole table
    T = GetDataFromTheTable(TheTable);

 else
    T = [];
    for j = 1:nargin
        curIn = varargin{j};
        if ischar(curIn)
            nn = isstrprop(curIn, 'alpha');
            curElem = curIn(nn);

            % Find the element symbol
            ie = strmatch(curElem, {TheTable{:, 1}}, 'exact');

            % Isotope mass number
            nn = isstrprop(curIn,  'digit');
            curMass = curIn;
            curMass(~nn) = ' ';
            curMass = strtok(curMass);
            if ~isempty(curMass)
                curMass = str2double(curMass);
                if ~isnan(curMass)
                    im = find(curMass == [TheTable{:, 4}]);
                    ie = intersect(ie, im);
                end
            end

            % Get the isotop(s) properties
            if isempty(ie)
                warning('MATLAB:isotopes:CanNotFindTheElement', ...
                        ['Can not find the element: ' curIn]);
                curT = GetDataFromTheTable;
                curT.name = '<< not found >>';
                curT.isotop = curIn;
                curT.mass = curMass;
                T = [T; curT(:)];
            else
                curT = GetDataFromTheTable(TheTable, ie);
                T = [T; curT(:)];
            end

        else
            error('Input argument(s) should be char, i.e. ''19F''');
        end
    end
end

%
function T = GetDataFromTheTable(TheTable, TblInd)
%Returns data in a structure

% Factor to Hz
fctHz = 1e6;

% Create the output structure template
StructTmpl = struct('name', '', 'isotop', '', 'Z', 0, 'mass', 0, 'I', 0, ...
                    'gamma', 0, 'ResFreq', 0, 'RelSensB', 0, ...
                    'RelSensF', 0, 'u', 0, 'Q', 0);
if nargin < 1
    T = StructTmpl;
    return;

elseif (nargin < 2)
    TblInd = (1:size(TheTable, 1));

elseif isempty(TblInd)
    T = StructTmpl;
    return;

end

nIndNum = length(TblInd);
T = repmat(StructTmpl, nIndNum, 1);
for i = 1:nIndNum
    j = TblInd(i);
    T(i).name = TheTable{j, 2};
    T(i).isotop = sprintf('%.0d%s', TheTable{j, 4}, TheTable{j, 1});
    T(i).Z = TheTable{j, 3};
    T(i).mass = TheTable{j, 4};
    T(i).I = TheTable{j, 5};
    T(i).gamma = 2*pi*fctHz*TheTable{j, 6};
    T(i).ResFreq = TheTable{j, 6}*fctHz;
    T(i).RelSensB = TheTable{j, 7};
    T(i).RelSensF = TheTable{j, 8};
    T(i).u = TheTable{j, 9};
    T(i).Q = TheTable{j, 10};
end


%PRM Probabilistic RoadMap navigation class
%
% A concrete subclass of the abstract Navigation class that implements the
% probabilistic roadmap navigation algorithm over an occupancy grid.  This
% performs goal independent planning of roadmaps, and at the query stage
% finds paths between specific start and goal points.
%
% Methods::
%
% plan         Compute the roadmap
% path         Compute a path to the goal
% visualize    Display the obstacle map (deprecated)
% plot         Display the obstacle map
% display      Display the parameters in human readable form
% char         Convert to string
%
% Example::
%
%        load map1              % load map
%        goal = [50,30];        % goal point
%        start = [20, 10];      % start point
%        prm = PRM(map);        % create navigation object
%        prm.plan()             % create roadmaps
%        prm.path(start, goal)  % animate path from this start location
%
% References::
%
% - Probabilistic roadmaps for path planning in high dimensional configuration spaces,
%   L. Kavraki, P. Svestka, J. Latombe, and M. Overmars,
%   IEEE Transactions on Robotics and Automation, vol. 12, pp. 566-580, Aug 1996.
% - Robotics, Vision & Control, Section 5.2.4,
%   P. Corke, Springer 2011.
%
% See also Navigation, DXform, Dstar, PGraph.

% 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

% Peter Corke 8/2009.

classdef PRM < Navigation

    properties
        npoints         % number of sample points
        distthresh      % distance threshold, links between vertices
                        % must be less than this.

        graph           % graph Object representing random nodes

        vgoal           % index of vertex closest to goal
        vstart          % index of vertex closest to start
        localGoal       % next vertex on the roadmap
        localPath       % set of points along path to next vertex
        gpath           % list of vertices between start and goal
    end

    methods

        % constructor
        function prm = PRM(varargin)
            %PRM.PRM Create a PRM navigation object
            %
            % P = PRM(MAP, options) is a probabilistic roadmap navigation
            % object, and MAP is an occupancy grid, a representation of a
            % planar world as a matrix whose elements are 0 (free space) or 1
            % (occupied).
            %
            % Options::
            %  'npoints',N      Number of sample points (default 100)
            %  'distthresh',D   Distance threshold, edges only connect vertices closer
            %                   than D (default 0.3 max(size(occgrid)))
            %
            % Other options are supported by the Navigation superclass.
            %
            % See also Navigation.Navigation.

            % invoke the superclass constructor, it handles some options
            prm = prm@Navigation(varargin{:});

            % create an empty 2D graph
            prm.graph = PGraph(2);

            % parse out PRM specific options and save in the navigation object
            opt.npoints = 100;
            opt.distthresh = 0.3*max(size(prm.occgrid));
            [opt,args] = tb_optparse(opt, varargin);
            prm.npoints = opt.npoints;
            prm.distthresh = opt.distthresh;
        end

        function plan(prm)
        %PRM.plan Create a probabilistic roadmap
        %
        % P.plan() creates the probabilistic roadmap by randomly
        % sampling the free space in the map and building a graph with
        % edges connecting close points.  The resulting graph is kept
        % within the object.

        % build a graph over the free space
            prm.message('create the graph');

            prm.graph.clear();  % empty the graph
            create_roadmap(prm);  % build the graph
        end

        function p = path(prm, start, goal)
        %PRM.path Find a path between two points
        %
        % P.path(START, GOAL) finds and displays a path from START to GOAL
        % which is overlaid on the occupancy grid.
        %
        % X = P.path(START) returns the path (2xM) from START to GOAL.

            if nargin < 3
                error('must specify start and goal');
            end

            % set the goal coordinate
            prm.goal = goal;

            % invoke the superclass path function, which iterates on our
            % next method
            if nargout == 0
                path@Navigation(prm, start);
            else
                p = path@Navigation(prm, start);
            end
        end

        % Handler invoked by Navigation.path() to start the navigation process
        %
        %   - find a path through the graph
        %   - determine vertices closest to start and goal
        %   - find path to first vertex
        function navigate_init(prm, start)

            % find the vertex closest to the goal
            prm.vgoal = prm.graph.closest(prm.goal);

            % find the vertex closest to the start
            prm.vstart = prm.graph.closest(start);

            % are the vertices connected?
            if prm.graph.component(prm.vstart) ~= prm.graph.component(prm.vgoal)
                error('PRM:plan:nopath', 'PRM: start and goal not connected: rerun the planner');
            end

            % find a path through the graph
            prm.message('planning path through graph');
            prm.graph.goal(prm.vgoal);   % set the goal
            prm.gpath = prm.graph.path(prm.vstart);

            % the path is a list of nodes from vstart to vgoal
            % discard the first vertex, since we plan a local path to it
            prm.gpath = prm.gpath(2:end);

            % start the navigation engine with a path to the nearest vertex
            prm.graph.highlight_node(prm.vstart);

            prm.localPath = bresenham(start, prm.graph.coord(prm.vstart));
            prm.localPath = prm.localPath(2:end,:);
        end

        % Invoked for each step on the path by path() method.
        function n = next(prm, p)

            if all(p(:) == prm.goal)
                n = [];     % signal that we've arrived
                return;
            end

            % we take the next point from the localPath
            if numrows(prm.localPath) == 0
                % local path is consumed, move to next vertex
                if isempty(prm.gpath)
                    % we have arrived at the goal vertex
                    % make the path from this vertex to the goal coordinate
                    prm.localPath = bresenham(p, prm.goal);
                    prm.localPath = prm.localPath(2:end,:);
                    prm.localGoal = [];
                else
                    % set local goal to next vertex in gpath and remove it from the list
                    prm.localGoal = prm.gpath(1);
                    prm.gpath = prm.gpath(2:end);

                    % compute local path to the next vertex
                    prm.localPath = bresenham(p, prm.graph.coord(prm.localGoal));
                    prm.localPath = prm.localPath(2:end,:);
                    prm.graph.highlight_node(prm.localGoal);
                end
            end

            n = prm.localPath(1,:)';     % take the first point
            prm.localPath = prm.localPath(2:end,:); % and remove from the path
        end

        function s = char(prm)
        %PRM.char  Convert to string
        %
        % P.char() is a string representing the state of the PRM
        % object in human-readable form.
        %
        % See also PRM.display.

            % invoke the superclass char() method
            s = char@Navigation(prm);

            % add PRM specific stuff information
            s = char(s, sprintf('  graph size: %d', prm.npoints));
            s = char(s, sprintf('  dist thresh: %f', prm.distthresh));
            s = char(s, char(prm.graph) );
        end

        function plot(prm, varargin)
        %PRM.plot Visualize navigation environment
        %
        % P.plot() displays the occupancy grid with an optional distance field.
        %
        % Options::
        %  'goal'            Superimpose the goal position if set
        %  'nooverlay'       Don't overlay the PRM graph

            opt.nooverlay = false;
            [opt,args] = tb_optparse(opt, varargin);

            % display the occgrid
            plot@Navigation(prm, args{:});

            if ~opt.nooverlay
                hold on
                prm.graph.plot()%varargin{:});
                hold off
            end
        end

    end % method

    methods (Access='protected')
    % private methods
        % create the roadmap
        function create_roadmap(prm)

            for j=1:prm.npoints
                % pick a point not in obstacle
                while true
                    x = prm.randi(numcols(prm.occgrid));
                    y = prm.randi(numrows(prm.occgrid));
                    if prm.occgrid(y,x) == 0
                        break;
                    end
                end
                new = [x; y];

                vnew = prm.graph.add_node(new);

                [d,v] = prm.graph.distances(new);
                % test neighbours in order of increasing distance
                for i=1:length(d)
                    if v(i) == vnew
                        continue;
                    end
                    if d(i) > prm.distthresh
                        continue;
                    end
                    if ~prm.testpath(new, prm.graph.coord(v(i)))
                        continue;
                    end
                    prm.graph.add_edge(vnew, v(i));
                end
            end
        end

        % test the path from p1 to p2 is entirely in free space
        function c = testpath(prm, p1, p2)
            p = bresenham(p1, p2);

            for pp=p'
                if prm.occgrid(pp(2), pp(1)) > 0
                    c = false;
                    return;
                end
            end
            c = true;
        end

    end % private methods

end % classdef


function f_x = ParFor5(in1)
%PARFOR5
%    F_X = PARFOR5(IN1)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    31-Mar-2020 16:52:33

r = in1(:,1);
s = in1(:,13);
u = in1(:,19);
ut = in1(:,20);
ux = in1(:,21);
uxx = in1(:,22);
uy = in1(:,23);
uyy = in1(:,24);
z = in1(:,7);
t2 = u.^2;
f_x = r.*-4.845016406907234+s.*6.96612441301113+ut.*5.457984690783633e-1-ux.*1.530748504418966e-2+uxx.*2.135644249305187-uy.*3.570878181824355e-2+uyy.*5.02823932467436-z.*1.323504040227272e2+r.*u.*6.191151455437648-s.*u.*9.105578580027213-t2.*u.*1.077295023277402e2+u.*ut.*5.65024831920482e-2+u.*ux.*1.830355084143775e-2-u.*uxx.*4.132096419489244+u.*uy.*4.442896435307375e-2-u.*uyy.*6.867807677976089+u.*z.*1.83605552916415e2+4.028421339788474e1;


function [ a, ipvt, rcond ] = chico ( a, lda, n )

%*****************************************************************************80
%
%% CHICO factors a complex hermitian matrix and estimates its condition.
%
%  Discussion:
%
%    If RCOND is not needed, CHIFA is slightly faster.
%
%    To solve A*X = B, follow CHICO by CHISL.
%
%    To compute inverse(A)*C, follow CHICO by CHISL.
%
%    To compute inverse(A), follow CHICO by CHIDI.
%
%    To compute determinant(A), follow CHICO by CHIDI.
%
%    To compute inertia(A), follow CHICO by CHIDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex A(LDA,N); on input, the hermitian matrix to be
%    factored.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, complex A(LDA,N); a block diagonal matrix and the multipliers
%    which were used to obtain it.  The factorization can be written
%    A = U*D*hermitian(U) where U is a product of permutation and unit
%    upper triangular matrices, hermitian(U) is the conjugate transpose
%    of U, and D is block diagonal with 1 by 1 and 2 by 2 blocks.
%    Only the diagonal and upper triangle are used.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, real RCOND, an estimate of the reciprocal condition of
%    the matrix.  For the system A*X = B, relative perturbations in A and B
%    of size EPSILON may cause relative perturbations in X of size
%    (EPSILON/RCOND).  If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%  Local Parameters:
%
%    Local, complex Z(N), a work vector whose contents are usually
%    unimportant.  If A is close to a singular matrix, then Z is an
%    approximate null vector in the sense that
%      norm(A*Z) = RCOND * norm(A) * norm(Z).
%

%
%  Find norm of A using only upper half.
%
  for j = 1 : n

    z(j) = scasum ( j, a(1:j,j), 1 );

    for i = 1 : j - 1
      z(i) = real ( z(i) ) + cabs1 ( a(i,j) );
    end

  end

  anorm = 0.0;
  for j = 1 : n
    anorm = max ( anorm, real ( z(j) ) );
  end
%
%  Factor.
%
  [ a, ipvt, info ] = chifa ( a, lda, n );
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A*Z = Y and A*Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where U*D*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve U*D*W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  k = n;

  while ( 0 < k )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    kp = abs ( ipvt(k) );
    kps = k + 1 - ks;

    if ( kp ~= kps )
      t      = z(kps);
      z(kps) = z(kp);
      z(kp)  = t;
    end

    if ( cabs1 ( z(k) ) ~= 0.0 )
      ek = csign1 ( ek, z(k) );
    end

    z(k) = z(k) + ek;
    z(1:k-ks) = z(1:k-ks) + z(k) * transpose ( a(1:k-ks,k) );

    if ( ks ~= 1 )

      if ( cabs1 ( z(k-1) ) ~= 0.0 )
        ek = csign1 ( ek, z(k-1) );
      end

      z(k-1) = z(k-1) + ek;
      z(1:k-ks) = z(1:k-ks) + z(k-1) * a(1:k-ks,k-1);

    end

    if ( ks ~= 2 )

      if ( cabs1 ( a(k,k) ) < cabs1 ( z(k) ) )
        s = cabs1 ( a(k,k) ) / cabs1 ( z(k) );
        z(1:n) = z(1:n) * s;
        ek = s * ek;
      end

      if ( cabs1 ( a(k,k) ) ~= 0.0 )
        z(k) = z(k) / a(k,k);
      else
        z(k) = 1.0;
      end

    else

      ak = a(k,k) / conj ( a(k-1,k) );
      akm1 = a(k-1,k-1) / a(k-1,k);
      bk = z(k) / conj ( a(k-1,k) );
      bkm1 = z(k-1) / a(k-1,k);
      denom = ak * akm1 - 1.0;
      z(k) = ( akm1 * bk - bkm1 ) / denom;
      z(k-1) = ( ak * bkm1 - bk ) / denom;

    end

    k = k - ks;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
%
%  Solve hermitian(U) * Y = W.
%
  k = 1;

  while ( k <= n )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= 1 )

      z(k) = z(k) + z(1:k-1) * conj ( a(1:k-1,k) );

      if ( ks == 2 )
        z(k+1) = z(k+1) + z(1:k-1) * conj ( a(1:k-1,k+1) );
      end

      kp = abs ( ipvt(k) );

      if ( kp ~= k )
        t     = z(k);
        z(k)  = z(kp);
        z(kp) = t;
      end

    end

    k = k + ks;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = 1.0;
%
%  Solve U*D*V = Y.
%
  k = n;

  while ( 0 < k )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= ks )

      kp = abs ( ipvt(k) );
      kps = k + 1 - ks;

      if ( kp ~= kps )
        t      = z(kps);
        z(kps) = z(kp);
        z(kp)  = t;
      end

      z(1:k-ks) = z(1:k-ks) + z(k) * transpose ( a(1:k-ks,k) );

      if ( ks == 2 )
        z(1:k-ks) = z(1:k-ks) + z(k-1) * a(1:k-ks,k-1);
      end

    end

    if ( ks ~= 2 )

      if ( cabs1 ( a(k,k) ) < cabs1 ( z(k) ) )
        s = cabs1 ( a(k,k) ) / cabs1 ( z(k) );
        z(1:n) = z(1:n) * s;
        ynorm = s * ynorm;
      end

      if ( cabs1 ( a(k,k) ) ~= 0.0 )
        z(k) = z(k) / a(k,k);
      else
        z(k) = 1.0;
      end

    else

      ak = a(k,k) / conj ( a(k-1,k) );
      akm1 = a(k-1,k-1) / a(k-1,k);
      bk = z(k) / conj ( a(k-1,k) );
      bkm1 = z(k-1) / a(k-1,k);
      denom = ak * akm1 - 1.0;
      z(k) = ( akm1 * bk - bkm1 ) / denom;
      z(k-1) = ( ak * bkm1 - bk ) / denom;

    end

    k = k - ks;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;
%
%  Solve hermitian(U) * Z = V.
%
  k = 1;

  while ( k <= n )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= 1 )

      z(k) = z(k) + z(1:k-1) * conj ( a(1:k-1,k) );

      if ( ks == 2 )
        z(k+1) = z(k+1) + z(1:k-1) *  conj ( a(1:k-1,k+1) );
      end

      kp = abs ( ipvt(k) );

      if ( kp ~= k )
        t     = z(k);
        z(k)  = z(kp);
        z(kp) = t;
      end

    end

    k = k + ks;

  end
%
%  Make ZNORM = 1.
%
  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


%% Pole Figure Tutorial
%
%%
% This tutorial explains the basic concepts for ananyzing x-ray, synchrotron
% and neutron diffraction pole figure data.
%
%% Import pole figure diffraction data
% Click on <matlab:import_wizard('PoleFigure') Import pole figure data> to
% start the import wizard which is a GUI leading you through the import of
% pole figure data. After finishing the wizard you will end up with a
% script similar to the following one.

% This script was automatically created by the import wizard. You should
% run the whole script or parts of it in order to import your data. There
% is no problem in making any changes to this script.
%
% *Specify Crystal and Specimen Symmetries*

% crystal symmetry for this ZnCuTi data is hexagonal.  Here we define the crystallographic unit cell and how it relates to cartesian xyz axes.
CS = crystalSymmetry('6/mmm', [2.633 2.633 4.8], 'X||a*', 'Y||b', 'Z||c');

% specimen symmetry tells MTEX if a certain symmetry should be present in the plotted pole figures.  The command used here selects triclinic, the most flexible option.
SS = specimenSymmetry('1');

% plotting convention
setMTEXpref('xAxisDirection','north');
setMTEXpref('zAxisDirection','outOfPlane');

%%
% *Specify File Names*

% path to files downloaded with the MTEX package
pname = [mtexDataPath filesep 'PoleFigure' filesep 'ZnCuTi' filesep];

% which pole figure files are to be imported
fname = {...
  [pname 'ZnCuTi_Wal_50_5x5_PF_002_R.UXD'],...
  [pname 'ZnCuTi_Wal_50_5x5_PF_100_R.UXD'],...
  [pname 'ZnCuTi_Wal_50_5x5_PF_101_R.UXD'],...
  [pname 'ZnCuTi_Wal_50_5x5_PF_102_R.UXD'],...
  };

% defocusing correction to compensate for the equipment-dependent loss of intensity at certain angles.
fname_def = {...
  [pname 'ZnCuTi_defocusing_PF_002_R.UXD'],...
  [pname 'ZnCuTi_defocusing_PF_100_R.UXD'],...
  [pname 'ZnCuTi_defocusing_PF_101_R.UXD'],...
  [pname 'ZnCuTi_defocusing_PF_102_R.UXD'],...
  };

%%
% *Specify Miller Indices*

% These correspond to the files loaded, in order.
h = { ...
  Miller(0,0,2,CS),...
  Miller(1,0,0,CS),...
  Miller(1,0,1,CS),...
  Miller(1,0,2,CS),...
  };

%%
% *Import the Data*

% create a Pole Figure variable containing the data
pf = PoleFigure.load(fname,h,CS,SS,'interface','uxd');

% create a defocusing pole figure variable
pf_def = PoleFigure.load(fname_def,h,CS,SS,'interface','uxd');

% correct data by applying the defocusing compensation
pf = correct(pf,'def',pf_def);

%%
% After running the script the variable |pf| is created which contains all
% information about the pole figure data. You may plot the data using the
% command <PoleFigure.plot.html plot>

plot(pf)

%%
% By default pole figures are plotted as intensity-colored dots for every
% data point. There are many options to specify the way pole figures are
% plotted in MTEX. Have a look at the <PoleFigurePlot.html plotting
% section> for more information.
%
% After import make sure that the Miller indices are correctly assigned to
% the pole figures and that the alignment of the specimen coordinate
% system, i.e., X, Y, Z is correct. In case of outliers or misaligned data,
% you may want to correct your raw data. Have a look at the
% <PoleFigureCorrection.html correction section> for further information.
% MTEX offers several methods correcting pole figure data, e.g.
%
% * rotating pole figures
% * scaling pole figures
% * finding outliers
% * removing specific measurements
% * superposing pole figures
%
% As an example we set all negative intensities to zero

pf(pf.intensities<0) = 0;
plot(pf)

%% ODF Estimation
%
% Once your data is in good shape, i.e. defocusing correction has been
% done and few outliers are left you can reconstruct an ODF out of
% this data. This is done by the command <PoleFigure.calcODF.html
% calcODF>.

odf = calcODF(pf,'silent')

%%
% Note that reconstructing an ODF from pole figure data is a severely ill-
% posed problem, i.e., it does *not* provide a unique solution. A more
% through discussion on the ambiguity of ODF reconstruction from
% pole figure data can be found <PoleFigure2ODFAmbiguity.html here>. As a
% rule of thumb: the more pole figures you have and the more consistent your
% pole figure data the better your reconstructed ODF will be.
%
% To check how well your reconstructed ODF fits the measured pole figure
% data use

figure;plotPDF(odf,pf.h)

%%
% Compare the recalculated pole figures with the measured data.
% A quantitative measure for the fitting is the so called RP value. They
% can be computed for each imported pole figure with

calcError(odf,pf)

%%
% In the case of a bad fit, you may want to tweak the reconstruction
% algorithm. See <PoleFigure2ODF.html here> for more information.

%% Visualize the ODF
% Finally one can plot the resulting ODF

plot(odf)
mtexColorMap LaboTeX


function tapas_hgf_plotTraj(r)
% Plots the estimated or generated trajectories for the HGF perceptual model
% Usage example:  est = tapas_fitModel(responses, inputs); tapas_hgf_plotTraj(est);
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 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/>.

% Optional plotting of standard deviations (true or false)
plotsd = true;

% Set up display
scrsz = get(0,'screenSize');
outerpos = [0.2*scrsz(3),0.2*scrsz(4),0.8*scrsz(3),0.8*scrsz(4)];
figure(...
    'OuterPosition', outerpos,...
    'Name', 'HGF trajectories');

% Time axis
if size(r.u,2) > 1 && ~isempty(find(strcmp(fieldnames(r.c_prc),'irregular_intervals'))) && r.c_prc.irregular_intervals
    t = r.u(:,end)';
else
    t = ones(1,size(r.u,1));
end

ts = cumsum(t);
ts = [0, ts];

% Number of levels
l = length(r.p_prc.p)/5;

% Upper levels
for j = 1:l-1

    % Subplots
    subplot(l,1,j);

    if plotsd == true
        upperprior = r.p_prc.mu_0(l-j+1) +sqrt(r.p_prc.sa_0(l-j+1));
        lowerprior = r.p_prc.mu_0(l-j+1) -sqrt(r.p_prc.sa_0(l-j+1));
        upper = [upperprior; r.traj.mu(:,l-j+1)+sqrt(r.traj.sa(:,l-j+1))];
        lower = [lowerprior; r.traj.mu(:,l-j+1)-sqrt(r.traj.sa(:,l-j+1))];

        plot(0, upperprior, 'ob', 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'ob', 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
    plot(ts, [r.p_prc.mu_0(l-j+1); r.traj.mu(:,l-j+1)], 'b', 'LineWidth', 2);
    hold all;
    plot(0, r.p_prc.mu_0(l-j+1), 'ob', 'LineWidth', 2); % prior
    xlim([0 ts(end)]);
    title(['Posterior expectation of x_' num2str(l-j+1)], 'FontWeight', 'bold');
    ylabel(['\mu_', num2str(l-j+1)]);
end

% Input level
subplot(l,1,l);

if plotsd == true
    upperprior = r.p_prc.mu_0(1) +sqrt(r.p_prc.sa_0(1));
    lowerprior = r.p_prc.mu_0(1) -sqrt(r.p_prc.sa_0(1));
    upper = [upperprior; r.traj.mu(:,1)+sqrt(r.traj.sa(:,1))];
    lower = [lowerprior; r.traj.mu(:,1)-sqrt(r.traj.sa(:,1))];

    plot(0, upperprior, 'or', 'LineWidth', 1);
    hold all;
    plot(0, lowerprior, 'or', 'LineWidth', 1);
    fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
end
plot(ts, [r.p_prc.mu_0(1); r.traj.mu(:,1)], 'r', 'LineWidth', 2);
hold all;
plot(0, r.p_prc.mu_0(1), 'or', 'LineWidth', 2); % prior
plot(ts(2:end), r.u(:,1), '.', 'Color', [0 0.6 0]); % inputs
if ~isempty(find(strcmp(fieldnames(r),'y'))) && ~isempty(r.y)
    plot(ts(2:end), r.y(:,1), '.', 'Color', [1 0.7 0]); % responses
    title(['Response y (orange), input u (green), and posterior expectation of x_1 ', ...
           '(red) for \rho=', num2str(r.p_prc.rho), ', \kappa=', ...
           num2str(r.p_prc.ka), ', \omega=', num2str(r.p_prc.om), ...
           ', \pi_u=', num2str(r.p_prc.pi_u)], ...
          'FontWeight', 'bold');
    ylabel('y, u, \mu_1');
else
    title(['Input u (green) and posterior expectation of x_1 ', ...
           '(red) for \rho=', num2str(r.p_prc.rho), ', \kappa=', ...
           num2str(r.p_prc.ka), ', \omega=', num2str(r.p_prc.om), ...
           ', \pi_u=', num2str(r.p_prc.pi_u)], ...
          'FontWeight', 'bold');
    ylabel('u, \mu_1');
end
xlim([0 ts(end)]);
xlabel({'Trial number', ' '}); % A hack to get the relative subplot sizes right
hold off;