function x = p06_start ( option, nvar )

%*****************************************************************************80
%
%% P06_START returns a starting point for problem 6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Input, integer NVAR, the number of variables.
%
%    Output, real X(NVAR), the starting point.
%
  x = zeros ( nvar, 1 );

  if ( option == 1 )

    x = [ ...
       0.000009180674117, ...
       0.051206082777927, ...
      -0.000003471121026, ...
       0.059606082627213, ...
       0.000016537664587, ...
      -0.050000000000000, ...
       0.000109431378424, ...
       0.000000000000000 ]';

  elseif ( option == 2 )

    x = [ ...
       0.000001548268247, ...
       0.008192973225663, ...
      -0.000000682134573, ...
       0.009536973221178, ...
       0.000002896734870, ...
      -0.008000000000000, ...
       0.000018188778989, ...
       0.000000000000000 ]';

  elseif ( option == 3 )

    x = [ ...
       0.0, ...
       0.0, ...
       0.0, ...
       0.0, ...
       0.0, ...
       0.0, ...
       0.0, ...
       0.0 ]';

  elseif ( option == 4 )

    x = [ ...
      -0.000010655314069, ...
      -0.051206082422980, ...
       0.000005600187501, ...
      -0.059606082643400, ...
      -0.000020891016199, ...
       0.050000000000000, ...
      -0.000122595323216, ...
       0.000000000000000 ]';

  elseif ( option == 5 )

    x = [ ...
      -0.000027083319493, ...
      -0.102412164106124, ...
       0.000014540858026, ...
      -0.119212165322433, ...
      -0.000048014067202, ...
       0.100000000000000, ...
      -0.000267808407544, ...
       0.000000000000000 ]';

  end

  return
end


function value = p06_param ( action, name, value )

%*****************************************************************************80
%
%% P06_PARAM gets or sets parameters for problem 6.
%
%  Discussion:
%
%    The parameter is named "M", and it represents the value of the exponent
%    in the integrand function:
%
%    Integral ( -oo < x < +oo ) x^m exp (-x*x) dx
%
%    M must be greater than -1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 May 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character ACTION, the action.
%    'S' to set the value,
%    'G' to get the value.
%
%    Input, character NAME, the parameter name.
%    'M', the exponent.
%
%    Input/output, integer VALUE, the parameter value.
%    If ACTION = 'S', then VALUE is an input quantity, and M is set to VALUE.
%    If ACTION = 'G', then VALUE is an output quantity, and VALUE is set to M.
%
  persistent m;

  if ( action == 'S' || action == 's' )

    if ( value <= -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'P06_PARAM - Fatal error!\n' );
      fprintf ( 1, '  Parameter M must be greater than -1.\n' );
      error ( 'P06_PARAM - Fatal error!' );
    end

    m = value;

  elseif ( action == 'G' || action == 'g' )

    value = m;

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'P06_PARAM - Fatal error!\n' );
    fprintf ( 1, '  Unrecognized value of ACTION = "%s".\n', action );
    error ( 'P06_PARAM - Fatal error!' );

  end

  return
end


%MDL_IR140 Create model of the ABB IRB 140 manipulator
%
%      mdl_irb140_mod
%
% Script creates the workspace variable irb which describes the
% kinematic characteristics of an ABB IRB 140 manipulator using
% modified DH conventions.
%
% Also define the workspace vectors:
%   qz         zero joint angle configuration
%
% Reference::
% - ABB IRB 140 data sheet
% - "THE MODELING OF A SIX DEGREE-OF-FREEDOM INDUSTRIAL ROBOT FOR
%   THE PURPOSE OF EFFICIENT PATH PLANNING"
%   Master of Science Thesis, Penn State U, May 2009
%   Tyler Carter
%
% See also SerialLink, mdl_irb140, mdl_puma560, mdl_stanford, mdl_twolink.
%
% Notes::
% - SI units of metres are used.
% - The tool frame is in the centre of the tool flange.
% - Zero angle configuration has the upper arm vertical and lower arm
%   horizontal.

% MODEL: ABB, IRB140, 6DOF, modified_DH

% Reference::

% 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

clear L

% joint angle limits from
% A combined optimization method for solving the inverse kinematics problem...
% Wang & Chen
% IEEE Trans. RA 7(4) 1991 pp 489-
L(1) = Revolute('d', 0.352, 'a', 0, 'alpha', 0, 'offset', 0, 'modified');
L(2) = Revolute('d', 0, 'a', 0.070, 'alpha', pi/2, 'offset', 0, 'modified');
L(3) = Revolute('d', 0, 'a', 0.360, 'alpha', 0, 'offset', 0, 'modified');
L(4) = Revolute('d', 0.380, 'a', 0, 'alpha', pi/2, 'offset', 0, 'modified');
L(5) = Revolute('d', 0, 'a', 0, 'alpha', -pi/2, 'offset', 0, 'modified');
L(6) = Revolute('d', 0, 'a', 0, 'alpha', pi/2, 'offset', 0, 'modified');

L(1).m = 34655.36e-3;
L(1).r = [27.87 43.12 -89.03]*1e-3;
L(1).I = [
    512052539.74 1361335.88 51305020.72
    1361335.88 464074688.59 70335556.04
    51305020.72 70335556.04 462745526.12]*1e-9;

L(2).m = 15994.59e-3;
L(2).r = [ 198.29 9.73 92.43]*1e03;
L(2).I = [
    94817914.40 -3859712.77 37932017.01
    -3859712.77 328604163.24 -1088970.86
    37932017.01 -1088970.86 277463004.88]*1e-9;

L(3).m = 20862.05e-3;
L(3).r = [ -4.56 -79.96 -5.86];
L(3).I = [
    500060915.95 -1863252.17 934875.78
    -1863252.17 75152670.69 -15204130.09
    934875.78 -15204130.09 515424754.34]*1e-9;

irb = SerialLink(L, 'name', 'IRB 140', ...
    'manufacturer', 'ABB', 'comment', 'modified DH');

%
% some useful poses
%
qz = [0 0 0 0 0 0]; % zero angles, L shaped pose

clear L


function [f] = Force(nnel,shape,P)

%--------------------------------------------------------------------------
% Purpose :
%         Determines the force vector for the element 'iel'
% Synopsis :
%          [f] = Force(nnel,shape,P)
% Variable Description:
%           f - element force vector
%           nnel - number of nodes per element
%           shape - Shape functions for the element
%           P - applied transverse pressure
%--------------------------------------------------------------------------

fef = shape*P ;

for i = 1:nnel
    i1=(i-1)*3+1;
    i2=i1+1;
    i3=i2+1;
    f(i1,1) = fef(1) ;
    f(i2,1) = 0 ;
    f(i3,1) = 0 ;
end


function roomhyp = sample_roomtype3(n, lines, vp, imgwidth, imgheight)

% 4 ways to create hypotheses
% roomtype1 + 3,top,left
% roomtype1 + 2,top
% roomtype6 + 3,bot,left
% roomtype6 + 2,bot

lc = [lines.lineclass]; % 0,1,2,3
lr = [lines.leftorright]; % 1 if right, -1 if left, 0 if neither
tb = [lines.above_horizon]; % 1 if above, -1 if below, 0 if neither

twotop = find((lc==2) & (tb==1));
twobot = find((lc==2) & (tb==-1));
threetopleft = find((lc==3) & (tb==1) & (lr==-1));
threebotleft = find((lc==3) & (tb==-1) & (lr==-1));

linecode{1} = twotop;
linecode{2} = twobot;
linecode{3} = threetopleft;
linecode{4} = threebotleft;

% recipe: [roomtypepart linecode oldcornerid newcornerid]
recipe(1,:) = [1 3 3 1];
recipe(2,:) = [1 1 3 1];
recipe(3,:) = [6 4 1 3];
recipe(4,:) = [6 2 1 3];

% roomhyp = [];
count = 0;
num_continue_without_progress = 0;
while count < n && num_continue_without_progress < 30
% pick recipe
    cur_recipe = recipe(randsample(size(recipe,1), 1), :);

    % sample partial room
    if cur_recipe(1) == 1
        roomhyppart = sample_roomtype1(1, lines, vp, imgwidth, imgheight);
    elseif cur_recipe(1) == 6
        roomhyppart = sample_roomtype6(1, lines, vp, imgwidth, imgheight);
    end

    if isempty(roomhyppart)
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end

    % sample lines
    l1 = linecode{cur_recipe(2)};
    if length(l1)<1
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end
    ls1 = l1(randsample(length(l1), 1));

    % new corner
    oldpoint = roomhyppart.corner(cur_recipe(3)).pt;
    [newpoint degen] = line_intersect(vp{1},oldpoint,...
        lines(ls1).point1, lines(ls1).point2);
    if degen==1
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end
    % check if inside img
    MARGIN = 5;
    if ~is_in_image(newpoint, imgwidth, imgheight, MARGIN)
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end

    % TODO: check location of new line sample
    %

    % add to hypothesis
    count = count + 1;
    num_continue_without_progress = 0;
    roomhyp(count) = roomhyppart;
    roomhyp(count).corner(cur_recipe(4)).pt = newpoint;
    roomhyp(count).type = 3;

%     global img;
%     disp_vanish(img, lines(ls1), vp);
%     plot(oldpoint(1), oldpoint(2), 'rx', 'MarkerSize',10, 'LineWidth',2);
%     plot(newpoint(1), newpoint(2), 'bx', 'MarkerSize',10, 'LineWidth',2);
%     pause;
%     close;
end

if ~exist('roomhyp','var')
    roomhyp = [];
end


function L = obslike_ehmm(ehmm,Gamma,residuals,XX,k)
%
% Evaluate likelihood of data given observation model
% for chain k, for one continuous trial
% (It's pretty much the same than obslike but simplified)
%
% INPUT
% X          N by ndim data matrix
% ehmm        ehmm data structure
% residuals  in case we train on residuals, the value of those.
% XX        alternatively to X (which in this case can be specified as []),
%               XX can be provided as computed by setxx.m
% OUTPUT
% B          Likelihood of N data points
%
% Author: Diego Vidaurre, University of Oxford / Aarhus University (2022)

K = ehmm.K;

[T,ndim] = size(residuals);
setstateoptions;
ltpi = sum(regressed)/2 * log(2*pi);
L = zeros(T+ehmm.train.maxorder,2);

ldetWishB = 0;
PsiWish_alphasum = 0;
for n = 1:ndim % only diagonal?
    if ~regressed(n), continue; end
    ldetWishB = ldetWishB+0.5*log(ehmm.Omega.Gam_rate(n));
    PsiWish_alphasum = PsiWish_alphasum+0.5*psi(ehmm.Omega.Gam_shape);
end
C = ehmm.Omega.Gam_shape ./ ehmm.Omega.Gam_rate;

for l = 1:2

    if l == 1, Gamma(:,k) = 1;
    else, Gamma(:,k) = 0;
    end

    [Xhat,XXstar] = computeStateResponses(XX,ehmm,Gamma,1:K+1);

    d = residuals(:,regressed) - Xhat(:,regressed);
    Cd = bsxfun(@times,C(regressed),d)';
    dist = zeros(T,1);
    for n = 1:sum(regressed)
        dist = dist - 0.5 * (d(:,n).*Cd(n,:)');
    end

    NormWishtrace = zeros(T,1);
%     if ndim == 1
%           NormWishtrace = NormWishtrace + 0.5 * C * ...
%                 sum( (XXstar * ehmm.state_shared(1).W.S_W) .* XXstar, 2);
%     else
%         for n = 1:ndim
%             if ~regressed(n), continue; end
%             Sind_all = repmat(Sind(:,n),K+1,1) == 1;
%             NormWishtrace = NormWishtrace + 0.5 * C(n) * ...
%                 sum( (XXstar(:,Sind_all) * ehmm.state_shared(n).S_W(Sind_all,Sind_all)) ...
%                 .* XXstar(:,Sind_all), 2);
%         end
%     end

    L(ehmm.train.maxorder+1:end,l) = - ltpi - ldetWishB + ...
        PsiWish_alphasum + dist - NormWishtrace;

end

Lneg = all(L<0,2);
if any(Lneg)
    L(Lneg,:) = L(Lneg,:) - repmat(max(L(Lneg,:),[],2),1,2);
end
%if any(sum(exp(L),2)==0), keyboard; end

L = exp(L);

end


function contrib_prev = reverse_conv(l_prev, contrib_curr, weights)

NON_NEG = true;
UNIT_LAYER = true;

new_weights = deconv_weights(weights);

    if NON_NEG && UNIT_LAYER
        new_weights(new_weights<0) = 0;
        l_prev(l_prev>0)=1;
        contrib_prev = my_vl_conv(contrib_curr, new_weights, []);
        contrib_prev = gather(gpuArray(contrib_prev) .* gpuArray(l_prev));
        contrib_prev = contrib_prev / max(abs(contrib_prev(:)));
    end

    if ~NON_NEG
        % For all positive elements in contrib_curr
        % positive weights means positive contribution
        % negative weights means negative contribution
        % so just do a normal conv pass with both positve and negative weights
        contrib_curr_pos = contrib_curr;
        contrib_curr_pos(contrib_curr_pos < 0) = 0;
        contrib_prev_pos = my_vl_conv(contrib_curr_pos, new_weights, []);
        ontrib_prev_pos = contrib_prev_pos .* l_prev;

        % For all non positive elements in contrib_curr
        % positive weights means negative contribution
        % negative weights means nothing (something that doesn't contribute to a negative pattern, can be positive or negative)
        % so we get the positive weights only to do the conv pass
        contrib_curr_neg = contrib_curr;
        contrib_curr_neg(contrib_curr_neg > 0) = 0;
        new_weights_pos = new_weights;
        new_weights_pos(new_weights_pos < 0) = 0;
        contrib_prev_neg = my_vl_conv(contrib_curr_neg, new_weights_pos, []);
        contrib_prev_neg = contrib_prev_neg .* l_prev;

        % Sum up the pos and neg results
        contrib_prev = contrib_prev_pos - sqrt((-1)*contrib_prev_neg);
        contrib_prev = contrib_prev / max(contrib_prev(:));
    end

end

    %{
    %{
    we can be sure that all l_prev is positive, and we don't need negative
    tensor flows to normalize the positive ones
    %}
    weights(weights<0) = 0;

    %normalize contrib_curr with its original input to get percentage
    %contribution

    %forward_pass = forward_conv(l_prev,weights);

    %to handle 0/0 case, we set all non-positive forward_pass element as -1
    %rationale being that we had removed all negative tensor flows by doing
    %weights(weights<0)=0, if the result is 0, then it must be originally
    %also 0 (because originally we didn't remove the negative flows so the
    %result will be negative, which is 0 after relu), so 0/0 will happen in
    %the element-wise division step below. To prevent this, set
    %non-positive elements in forward_pass to -1 so that it becomes 0/-1.
    %The answer is 0 which is the desired behavior (because 0% contribution
    %is what it deserves)
    % that way

    %forward_pass(forward_pass<=0) = -1;
    %contrib_curr = contrib_curr ./ forward_pass;

    %}
    %contrib_curr = contrib_curr .* sqrt(contrib_curr);
    %contrib_curr = contrib_curr / sum(contrib_curr(:));


%> @brief Convert body struct to a spatial_v2 model
%> The purose of this is to facility an ease of conversion between
%> spatial_v2 and RBDL via files read in by yaml
function [model] = body_struct_to_spatial_model(robot, gravity)

if nargin < 2
    gravity = -9.81;
end

% dim = body_struct.dim; % @todo Add in option for planar
bodies = robot.Joints;
NB = length(bodies);
blankc = cell(NB, 1);
blankm = zeros(NB, 1);

model = struct();
% Fields for spatial_v2
model.type = 'spatial';
model.NB = NB;
model.parent = blankm;
model.jtype = blankc;
model.Xtree = blankc;
model.I = blankc;
model.mass = blankm;
model.gravity = [0, 0, gravity];

% Additional fields for kinematics
model.name = blankc;
model.axis_index = blankm;
model.is_revolute = blankm;
axes = 'xyz';

for i = 1:NB
    body = bodies(i);
    reference = body.Reference;
    lambda = getJointIndices(robot, reference.Name);

    model.parent(i) = lambda;

    index = find(body.Axis);
    assert(isscalar(index));
    axis_value = body.Axis(index);

    if strcmpi(body.Type,'revolute') || strcmpi(body.Type,'continuous')
        model.is_revolute(i) = true;
        prefix = 'R';
    else
        prefix = 'P';
    end
    if axis_value < 0
        jtype = ['-', prefix, axes(index)];
    else
        jtype = [prefix, axes(index)];
    end

    model.jtype{i} = jtype;
    model.Xtree{i} = plux(body.R, body.Offset);

    child_link_idx = getLinkIndices(robot, body.Child);

    if ~isnan(child_link_idx)
        link = robot.Links(child_link_idx);
        model.I{i} = mcI(link.Mass, link.Offset, link.Inertia);
        model.mass(i) = link.Mass;
    else
        model.I{i} = mcI(0, zeros(3,1), zeros(3));
        model.mass(i) = 0;
    end
    model.name{i} = body.Name;
    model.axis_index(i) = index;
end

end


function [ferns,hsPr] = fernsClfReplaceFeature( data, hs, ferns, features_remove, varargin )
% Train random fern classifier.
%
% See "Fast Keypoint Recognition in Ten Lines of Code" by Mustafa Ozuysal,
% Pascal Fua and Vincent Lepetit, CVPR07.
%
% Dimensions:
%  M - number ferns
%  S - fern depth
%  F - number features
%  N - number input vectors
%  H - number classes
%
% USAGE
%  [ferns,hsPr] = fernsClfTrain( data, hs, [varargin] )
%
% INPUTS
%  data     - [NxF] N length F feature vectors
%  hs       - [Nx1] target output labels in [1,H]
%  varargin - additional params (struct or name/value pairs)
%   .S        - [10] fern depth (ferns are exponential in S)
%   .M        - [50] number of ferns to train
%   .thrr     - [0 1] range for randomly generated thresholds
%   .bayes    - [1] if true combine probs using bayes assumption
%
% OUTPUTS
%  ferns    - learned fern model w the following fields
%   .fids     - [MxS] feature ids for each fern for each depth
%   .thrs     - [MxS] threshold corresponding to each fid
%   .pFern    - [2^SxHxM] learned log probs at fern leaves
%   .bayes    - if true combine probs using bayes assumption
%   .inds     - [NxM] cached indices for original training data
%   .H        - number classes
%  hsPr     - [Nx1] predicted output labels
%
% EXAMPLE
%  N=5000; H=5; d=2; [xs0,hs0,xs1,hs1]=demoGenData(N,N,H,d,1,1);
%  fernPrm=struct('S',4,'M',50,'thrr',[-1 1],'bayes',1);
%  tic, [ferns,hsPr0]=fernsClfTrain(xs0,hs0,fernPrm); toc
%  tic, hsPr1 = fernsClfApply( xs1, ferns ); toc
%  e0=mean(hsPr0~=hs0); e1=mean(hsPr1~=hs1);
%  fprintf('errors trn=%f tst=%f\n',e0,e1); figure(1);
%  subplot(2,2,1); visualizeData(xs0,2,hs0);
%  subplot(2,2,2); visualizeData(xs0,2,hsPr0);
%  subplot(2,2,3); visualizeData(xs1,2,hs1);
%  subplot(2,2,4); visualizeData(xs1,2,hsPr1);
%
% See also fernsClfApply, fernsInds
%
% Piotr's Image&Video Toolbox      Version 2.50
% Copyright 2010 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Lesser GPL [see external/lgpl.txt]

% get additional parameters and check dimensions
dfs={'features_replace',[],'doreplace',[],'fids_replace',[]};
[features_replace,doreplace,fids_replace]=getPrmDflt(varargin,dfs,1);
[N,F]=size(data); assert(length(hs)==N);
M=size(ferns.inds,2); S=log2(size(ferns.pFern,1));
H=max(hs); assert(all(hs>0)); assert(S<=20);
assert(size(ferns.inds,1)==N);

% indices we want to remove
if isempty(doreplace),
  doreplace = ismember(ferns.fids,features_remove);
else
  assert(numel(doreplace)==M*S);
end
nreplace = nnz(doreplace);

% features to replace
if isempty(fids_replace),
  % allowed features to replace with
  if isempty(features_replace),
    features_replace = uint32(setdiff(1:F,features_remove));
  else
    features_replace = uint32(features_replace);
  end
  fids_replace = features_replace(floor(rand(nreplace,1)*numel(features_replace)+1));
else
  assert(numel(fids_replace)==nreplace);
end

% replace features in fids
ferns.fids(doreplace) = fids_replace;

% update inds to reflect these replacements
ferns.inds = fernsInds_update(data,ferns.fids,ferns.thrs,ferns.inds,doreplace);

% get counts for each leaf for each class for each fern
% KB: use hist to avoid inner loop
pFern = nan(2^S,H,M);
edges = 1:2^S;
for m = 1:M,
  for h = 1:H,
    pFern(:,h,m) = histc(ferns.inds(hs==h,m),edges);
  end
end
pFern = pFern + ferns.bayes;

% KB: store the unnormalized counts
ferns.counts = pFern;

% convert fern leaf class counts into probabilities
if( ferns.bayes<=0 )
  pFern = bsxfun(@rdivide,pFern,sum(pFern,2));
else
  pFern = bsxfun(@rdivide,pFern,sum(pFern,1));
  pFern=log(pFern);
end

% store pFern and compute output values
ferns.pFern=pFern; clear pFern;
if(nargout==2), hsPr=fernsClfApply([],ferns,ferns.inds); end

end


function ori = transformReferenceFrame(ori,cs1,cs2)
% change reference frame of an orientation
%
% Orientations are always described with respect to a cartesian reference
% frame x, y, z aligned in a fixed way with the crystal coordiante system
% a, b, c. Typical alignments are x||a and z||c* or x||a* and z||c. This
% function allows to change the aligment of the reference frame while NOT
% changing the orientation.
%
% Syntax
%   ori = ori.transformReferenceFrame(cs)
%   mori = mori.transformReferenceFrame(cs1,cs2)
%
% Input
%  ori - @orientation
%  mori - misorientation
%  cs, cs1, cs2 - @crystalSymmetry
%

% only applicable for crystal symmetry
if ~isa(cs1,'crystalSymmetry')
  warning('Symmetry missmatch!')
  ori.CS = cs1;
  return
end

% basis transformation into reference frame
M = transformationMatrix(ori.CS,cs1);

% check symmetries are compatible
if ori.CS.id ~= cs1.id || norm(eye(3)-M*M.')>0.01 || ...
    all(norm(ori.CS.axes - cs1.axes)./norm(cs1.axes)<10^-2) || ...
    (~isempty(ori.CS.mineral) && ~isempty(cs1.mineral) && ~strcmpi(ori.CS.mineral,cs1.mineral))
  warning('Symmetry missmatch! The following crystal frames seem to be different\n\n  %s\n  %s \n',char(ori.CS,'verbose'),char(cs1,'verbose'));
end

if det(M)>10*eps
  ori = times(ori, rotation('matrix',M^-1), 0);
  ori.CS = cs1;
end

% do the same for the second symmetry
if nargin == 3, ori = inv(transformReferenceFrame(inv(ori),cs2)); end

% this is some testing code
% cs1 = crystalSymmetry('triclinic',[1 2 3],[70 80 120]*degree,'Z||a*')
% cs2 = crystalSymmetry('triclinic',[1 2 3],[70 80 120]*degree,'Z||b','X||a*')
% o = orientation.byEuler(30*degree,50*degree,120*degree,cs1)
% o * Miller(1,0,0,cs1)
% o2 = transformReferenceFrame(o,cs2)
% o2 * Miller(1,0,0,cs2)


function [ y, j, f ] = jed_to_yjf_roman ( jed )

%*****************************************************************************80
%
%% JED_TO_YJF_ROMAN converts a JED to a Roman YJF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer Y, J, real F, the YJF date.
%
  [ y1, m1, d1, f1 ] = jed_to_ymdf_roman ( jed );

  [ y, j, f ] = ymdf_to_yjf_roman ( y1, m1, d1, f1 );

  return
end


function outname = writeww3(outfiname,EToV,VX,B,opedat, title)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function writes WW3 grid including nodes             %
% (longitude,latitude,depth), open bounday nodes            %
% and element connections (triangles)                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INPUT:                                                    %
% EToV: triangle (nelem,3);                                 %
% VX(:,1): longitude (nnode,1);                             %
% VX(:,2): laritude (nnode,1);                              %
% B: depth (nnode,1);                                       %
% opedat.nbdv: Open boundary nodes ID                       %
% outfiname: mesh name                                      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      Ali Abdolali August 2018 ali.abdolali@noaa.gov       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('write WW3 msh') ;

node(:,1)=(1:length(VX(:,1)));
node(:,2)=VX(:,1);
node(:,3)=VX(:,2);
node(:,4)=B;

fid = fopen(outfiname,'w');
outname = outfiname ;
disp( title )  ;
fprintf(fid,'%s\n', '$MeshFormat');
fprintf(fid,'%s\n', '2 0 8');
fprintf(fid,'%s\n', '$EndMeshFormat');
fprintf(fid,'%s\n', '$Nodes');
fprintf(fid,'%d\n', length(node(:,1)));
   for i=1:length(node(:,1))
        fprintf(fid,['%d %s %5.5f %s %5.5f %s %5.5f\n'], node(i,1),'', VX(i,1),'',VX(i,2),'',B(i,1));
    end
fprintf(fid,'%s\n', '$EndNodes');
fprintf(fid,'%s\n', '$Elements');
fprintf(fid,'%d\n', length(EToV(:,1))+opedat.neta);
m=0;
for i=1:opedat.nope
    for j=1:opedat.nvdll(i)
        m=m+1;
        fprintf(fid,['%d %s %d %s %d %s %d %s %d %s %d\n'], m,'',15,'',2,'',0,'',0,'',opedat.nbdv(j,i));
    end
end
for i=1:length(EToV(:,1))
   m=m+1;
   fprintf(fid,['%d %s %d %s %d %s %d %s %d %s %d %s %d %s %d %s %d\n'], m,'',2,'',3,'',0,'',i,'',0,'',EToV(i,1),'',EToV(i,2),'',EToV(i,3));
end
fprintf(fid,'%s', '$EndElements');
fclose(fid) ;
return


function SO3F = plus(SO3F1,SO3F2)
% overloads |SO3F1 + SO3F2|
%
% Syntax
%   SO3F = SO3F1 + SO3F2
%   SO3F = a + SO3F1
%   SO3F = SO3F1 + a
%
% Input
%  SO3F1, SO3F2 - @SO3Fun
%  a - double
%
% Output
%  SO3F - @SO3Fun
%

% uniform component + SO3Fun
if isa(SO3F1,'SO3FunRBF') && isempty(SO3F1.center) && isa(SO3F2,'SO3Fun')
  SO3F = SO3F1.c0 + SO3F2;
  return
end
if isa(SO3F2,'SO3FunRBF') && isempty(SO3F2.center) && isa(SO3F1,'SO3Fun')
  SO3F = SO3F1 + SO3F2.c0;
  return
end

ensureCompatibleSymmetries(SO3F1,SO3F2);
SO3F = SO3FunComposition(SO3F1, SO3F2);

end


function test_issue1184

% MEM 8gb
% WALLTIME 00:30:00
% DEPENDENCY ft_resampledata

global ft_default
ft_default.toolbox.signal = 'matlab'; % do not use the compat version, its resample/firls is too slow
clear ft_defaults % ensure the path setup to be repeated

data = [];
data.time{1} = (1:228864) * 1/1200;
data.trial{1}(1,:) = 1:228864;
data.label = {'time'};
data.fsample = 1200;

cfg = [];
cfg.resamplefs = 100;
resampled = ft_resampledata(cfg, data);

assert(length(resampled.time{1})==19072);

%%

data11 = [];
data11.time{1} = (1:11) * 1/1200;
data11.trial{1}(1,:) = 1:11;
data11.label = {'time'};
data11.fsample = 1200;

cfg = [];
cfg.resamplefs = 100;
resampled11 = ft_resampledata(cfg, data11);

assert(isalmostequal(resampled11.time{1}(1), mean(data11.time{1}), 'abstol', 1e-6));

data12 = [];
data12.time{1} = (1:12) * 1/1200;
data12.trial{1}(1,:) = 1:12;
data12.label = {'time'};
data12.fsample = 1200;

cfg = [];
cfg.resamplefs = 100;
resampled12 = ft_resampledata(cfg, data12);

assert(isalmostequal(resampled12.time{1}(1), mean(data12.time{1}), 'abstol', 1e-6));

%%

% resampled11 and resampled12 have a time axis that is shifted by half a sample (at 1200 Hz)
% since they are constructed from two data structures that are different by one sample
%
% the following code would result in a data structure in which the trials are slightly mis-aligned
% resampledXX = ft_appenddata([], resampled11, resampled12);

% when resampling both trials at the same time, the time axes should remain consistent
dataXX = ft_appenddata([], data11, data12);

for resamplefs=1:1200
  cfg = [];
  cfg.showcallinfo = 'no';
  cfg.resamplefs = resamplefs;
  resampledXX = ft_resampledata(cfg, dataXX);

  % with a resampling rate of 600 or higher, the time axes of the resampled trials will have a different length due to the padding
  % only the first part should be compared
  n = min(length(resampledXX.time{1}), length(resampledXX.time{2}));
  assert(isalmostequal(mean(resampledXX.time{1}(1:n)), mean(resampledXX.time{2}(1:n)), 'abstol', 1e-6));
end


function [u,flag,relres,iter,resvec] = minres(L,f,tol,maxit,R1,R2,u0,varargin)
%MINRES    A Preconditioned Minimal Residual method for ODEs.
%   U = MINRES(L,F) attempts to solve the linear ODE L(U) = F on DOMAIN(L) with
%   boundary conditions. The CHEBOP L must be a self-adjoint, uniformly elliptic
%   second-order differential operator of the form
%          L(U) = (a(x)*U')' + c(x)U,
%   with dirichlet boundary conditions. The righthand side F should be a chebfun
%   on DOMAIN(L). By default the indefinite integral operator is employed as a
%   preconditioner.
%
%   U = MINRES(L,F,TOL) specifies the tolerance of the method. If TOL is [] then
%   MINRES uses the default in cheboppref.
%
%   U = MINRES(L,F,TOL,MAXIT) specifies the maximum number of iterations. If
%   MAXIT is [] then MINRES uses the default in cheboppref.
%
%   U = MINRES(L,F,TOL,MAXIT,R1,R2) solves the preconditioned linear ODE of
%   (R2*L*R1)(V) = R2*f, where R2 must be the adjoint of R1. R1 and R2 must be
%   function handles. If R1 = [], then the default preconditioner is employed.
%   Only the default preconditioner is currently supported.
%
%   U = MINRES(L,F,TOL,MAXIT,R1,R2,U0) specifies the initial guess. If U0 is []
%   then MINRES MINRES the default, the zero function on DOMAIN(L).
%
%   [U,FLAG] = MINRES(L,F,...) also returns a convergence FLAG:
%    0 MINRES converged to the desired tolerance TOL within MAXIT iterations
%    1 MINRES iterated MAXIT times but did not converge.
%    2 preconditioner R1 was an unbounded operator.
%    3 MINRES stagnated (two consecutive iterates were the same).
%    4 one of the scalar quantities calculated during MINRES became too
%      small or too large to continue computing.
%
%   [U,FLAG,RELRES] = MINRES(L,F,...) also returns the relative residual
%    NORM(R2(F)-R2(L(U)),2)/NORM(F,2). If FLAG is 0, then RELRES <= TOL.
%
%   [U,FLAG,RELRES,ITER] = MINRES(L,F,...) also returns the iteration number at
%   which U was computed: 0 <= ITER <= MAXIT.
%
%   [U,FLAG,RELRES,ITER,RESVEC] = MINRES(L,F,...) returns a vector of estimated
%   residual norms at each iteration including NORM(R2(F)-R2(L(Uk)),2).
%
% See also CHEBOP/PCG and CHEBOP/GMRES.

% Copyright 2017 by The University of Oxford and The Chebfun Developers. See
% http://www.chebfun.org/ for Chebfun information.

% Only continue if L is a linear chebop:
if ( ~all(islinear(L)) )
    error('CHEBFUN:CHEBOP:pcg:nonlinear', ...
        'MINRES supports only linear CHEBOP instances.');
end

% At the moment, we need a second-order differential equation:
LinearOp = linop( L );
if ( LinearOp.diffOrder ~=2 )
    error('CHEBFUN:CHEBOP:pcg:DiffOrder', ...
        'MINRES supports only second-order ODEs.');
end

if ( nargin < 2 )
    error(message('chebop:minres:NotEnoughInputs'));
end

% Norm of righthand side function:
n2f = norm(f, 2);
n = length( f );
dom = domain( f );

% Grab boundary conditions from chebop:
if ( isa( L, 'chebop' ) )
    if isempty(L.lbc)
        left_bc = 0;
    else
        left_bc = L.lbcShow;

        % Current implementation requires Dirichlet boundary conditions:
        if ( ~isa(left_bc, 'double' ) )
            error('CHEBFUN:CHEBOP:pcg:leftbc', ...
                   'Currently, we require Dirichlet boundary conditions. Please supply N.lbc = double.');
        end
    end

    if ( isempty(L.rbc) )
        right_bc = 0;
    else
        right_bc = L.rbcShow;
        % Current implementation requires Dirichlet boundary conditions:
        if ( ~isa(right_bc, 'double' ) )
            error('CHEBFUN:CHEBOP:pcg:rightbc', ...
                   'Currently, we require Dirichlet boundary conditions. Please supply N.rbc = double.');
        end
    end

    L = L.op;
elseif ( isa( L, 'function_handle' ) )
    left_bc = 0;
    right_bc = 0;
else
    error(message('chebop:pcg:DiffOperatorIllDefined'));
end

% Ensure that L = @(x,u) ...
if ( nargin(L) == 1 )
    L = @(x,u) L(u);
elseif ( nargin(L) ~=2 )
    error(message('chebop:pcg:DiffOpNargin'));
end

% Data mine L for faster "mat-vec" computations in the CG iteration:
x = chebfun( @(x) x );
c = L(x, 1+0*x);
a = -L(x, x.^2/2) - (-L(x, x) + c.*x).*x + c.*(x.^2/2);
L = @(v) -diff( a.*diff( v ) ) + c.*v;

% Assign default values to unspecified parameters
if ( (nargin < 3) || isempty(tol) )
    tol = cheboppref().bvpTol;
end
warned = 0;
if ( tol <= eps )
    warning(message('chebop:pcg:tooSmallTolerance'));
    warned = 1;
    tol = eps;
elseif ( tol >= 1 )
    warning(message('chebop:pcg:tooBigTolerance'));
    warned = 1;
    tol = 1-eps;
end
if ( (nargin < 4) || isempty(maxit) )
    maxit = cheboppref().maxIter;
end

if ( (nargin < 5) || isempty(R1) )
    % Preconditioner:
    R1 = @(u) cumsum(u);
else
    % The method only works with the default preconditioner:
    error(message('chebop:pcg:OnlyDefaultPreconditionerAllowed'))
end

if ( (nargin < 6) || isempty(R2) )
    % Adjoint operator to R1:
    R2 = @(u) sum(u) - cumsum(u);
else
    % The method only works with the default preconditioner:
    error(message('chebop:pcg:OnlyDefaultPreconditionerAllowed'))
end

% Projection operator
Pi = @(g) g - mean(g);

% Build modified differential operator:
x = chebfun( @(x) x, dom );
T = @(v) Pi( R2( L( R1( v ) ) ) );

if ( (nargin >= 7) && ~isempty(u0) )
    if ( ~domainCheck(f, u0) )
        error(message('chebop:pcg:WrongInitGuessDomain'));
    else
        u = u0;
    end
    Tu = T(u);
else
    % Otherwise, initial guess is the zero function:
    u = 0*f;
    Tu = u;
end

if ( nargin > 7 )
    error(message('chebop:minres:TooManyInputs'));
end

% Ensure that rhs is within the correct space, and if not then solve a modified
% problem.
R2f = R2( f );
PiR2f = Pi( R2f );
if ( norm( R2f - PiR2f ) > tol || norm(left_bc)> tol || norm(right_bc) > tol )
    % f is NOT in the space Wn = {v in L_2: R1(v) in V_{n,0} }:
    basis = x.^(0:4);
    A = zeros(4, size(basis,2));
    for jj = 1:size(basis,2)
        A(1:2,jj) = feval( R1( R2( L(basis(:,jj)) ) ), dom([1,length(dom)])');
        A(3:4,jj) = feval(basis(:,jj),dom([1,length(dom)])');
    end
    b = [ feval( R1( R2f ), dom([1,length(dom)])') ; left_bc ; right_bc];
    warning('off','all')
    z = basis*(A\b);
    warning('on','all');
    % g is now in the space Wn = {v in L_2: R1(v) in V_{n,0} }:
    g = Pi( R2f - R2( L(z) ) );
else
    % f is in the space Wn = {v in L_2: R1(v) in V_{n,0} }:
    g = PiR2f;
    z = 0*f;
end

% Set up for the method
flag = 1;
iter = 0;
umin = u;                          % Iterate which has minimal residual so far
imin = 0;                          % Iteration at which xmin was computed
tolg = tol * normest(g);           % Relative tolerance
r = g - Tu;
normr = norm(r,2);                 % Norm of residual
normr_act = normr;

if ( normr <= tolg )               % Initial guess is a good enough solution
    flag = 0;
    relres = normr / n2f;
    resvec = normr;
    % Undo preconditioner:
    u = R1( Pi( u ) );
    return
end

resvec = zeros(maxit+1,1);         % Preallocate vector for MINRES residuals
resvec(1) = normr;                 % resvec(1) = norm(b-A*x0)
normrmin = normr;                  % Norm of minimum residual

vold = r;
v = vold;
beta1 = innerProduct(vold, v);
if ( beta1 <= 0 )
    flag = 5;
    relres = normr / n2b;
    resvec = resvec(1);
    u = R1( Pi( u ) );
    return
end
beta1 = sqrt(beta1);
snprod = beta1;
vv = v / beta1;
v = T(vv);
Amvv = v;
alpha = innerProduct(vv,v);
v = v - (alpha/beta1) * vold;

% Local reorthogonalization
numer = innerProduct(vv, v);
denom = innerProduct(vv, vv);
v = v - (numer/denom) * vv;
volder = vold;
vold = v;
betaold = beta1;
beta = innerProduct(v,v);
if ( beta < 0 )
    flag = 5;
    relres = normr / n2b;
    resvec = resvec(1);
    u = R1( Pi( u ) );
    return
end
iter = 1;
beta = sqrt(beta);
gammabar = alpha;
epsilon = 0;
deltabar = beta;
gamma = sqrt(gammabar^2 + beta^2);
mold = chebfun('0');
Amold = mold;
m = vv / gamma;
Am = Amvv / gamma;
cs = gammabar / gamma;
sn = beta / gamma;
u = u + snprod * cs * m;
snprod = snprod * sn;

normr = abs(snprod);
resvec(2,1) = normr;

% Check for convergence after first step.
if ( normr <= tolg )
    flag = 0;
    relres = normr / n2f;
    resvec = resvec(1:2);
    u = R1( Pi( u ) );
    return
end

stag = 0;                          % stagnation of the method
moresteps = 0;
maxmsteps = min([floor(n/50),5,n-maxit]);
maxstagsteps = 3;

% loop over maxit iterations (unless convergence or failure)
for ii = 2:maxit

    vv = v * (1/beta);
    v = T(vv);
    Amolder = Amold;
    Amold = Am;
    Am = v;
    v = v - (beta / betaold) * volder;
    alpha = innerProduct(vv, v);
    v = v - (alpha / beta) * vold;
    volder = vold;
    vold = v;
    betaold = beta;
    beta = innerProduct(v, v);
    if (beta < 0)
        flag = 5;
        break
    end
    beta = sqrt(beta);
    delta = cs * deltabar + sn * alpha;
    molder = mold;
    mold = m;
    m = vv - delta * mold - epsilon * molder;
    Am = Am - delta * Amold - epsilon * Amolder;
    gammabar = sn * deltabar - cs * alpha;
    epsilon = sn * beta;
    deltabar = - cs * beta;
    gamma = sqrt(gammabar^2 + beta^2);
    m = m / gamma;
    Am = Am / gamma;
    cs = gammabar / gamma;
    sn = beta / gamma;
    % Check for stagnation of the method
    if (snprod*cs == 0) || (abs(snprod*cs)*norm(m) < eps*norm(u))
        % increment the number of consecutive iterates which are the same
        stag = stag + 1;
    else
        stag = 0;
    end
    u = u + (snprod * cs) * m;
    snprod = snprod * sn;
    normr = abs(snprod);

    resvec(ii+1,1) = normr;

    % Check for convergence
    if ( ( normr <= tolg ) || ( stag >= maxstagsteps ) || moresteps)
        % double check residual norm is less than tolerance
        r = g - T( u );
        normr_act = norm(r);
        resvec(ii+1, 1) = normr_act;
        if (normr_act <= tolg)
            flag = 0;
            iter = ii;
            break
        else
            if stag >= maxstagsteps && moresteps == 0
                stag = 0;
            end
            moresteps = moresteps + 1;
            if moresteps >= maxmsteps
                if ~warned
                    warning('tooSmallTolerance');
                end
                flag = 3;
                iter = ii;
                break;
            end
        end
    end

    if ( normr < normrmin )        % update minimal norm quantities
        normrmin = normr;
        umin = u;
        imin = ii;
    end

    if ( stag >= maxstagsteps )    % 3 iterates are the same
        flag = 3;
        break
    end
end                                % for ii = 1 : maxit
if ( isempty(ii) )
    ii = 1;
end

% Returned solution is first with minimal residual:
if ( flag == 0 )
    relres = normr_act / n2f;
else
    r_comp = g - T(u);
    if ( norm(r_comp) <= normr_act )
        u = umin;
        iter = imin;
        relres = norm(r_comp) / n2f;
    else
        iter = ii;
        relres = normr_act / n2f;
    end
end

% Undo preconditioner:
u = R1( u ) + z;

% truncate the zeros from resvec
if ( (flag <= 1) || (flag == 3) )
    resvec = resvec(1:ii+1);
else
    resvec = resvec(1:ii);
end

end


function [c, s, kernel, iter] = deconvCa(y, kernel, smin, fit_gt, debug_on, sn, maxIter, theta, lambda)
%% deconvolve calcium traces to infer spike counts
%% inputs:
%   y:  1*T vector, observed calcium traces
%   kernel: struct variable with two fields {'fhandle', 'pars', nMax}. kernel
%   deterines the convolution kernel
%      fhandle: function handle, the function form of the response function
%       pars: p*1 vector, parameters for fhandle
%       nMax: scalar, maximum number of frames required by calcium indicators
%           to return resting states
%   smin: scalar, minimum number of nonzero spike count within each bin
%   fit_gt: true or false. iteratively fit gt or not
%   debug_on: play and save video of the whole procedure.
%   sn:     noise power
%   maxIter: scalar, maximum iterations for running OASIS, default (3)
%   theta: 1*T vecotr,  weight vector at each time point
%   lambda: scalar, tuning parameter

%% outputs:
%   c: 1*T vector, inferred calcium trace
%   s: 1*T vector, inferred spike train
%   kernel: convolution kernel
%   iters: number of iterations for updating irf

%% Author: Pengcheng Zhou, Carnegie Mellon University, 2016
% This work is based on one NIPS paper by Johannes Friedrich & Liam
% Paninski

%% input arugments
y = reshape(y, 1, []);  % convert data into row vector
if ~exist('sn', 'var') || isempty(sn)
    sn = get_noise_fft(y);
end
T = length(y);          % number of frames

% get convolution kernel
if ~exist('kernel', 'var') || isempty(kernel)
    kernel = create_kernel('exp2', ar2exp(estimate_time_constant(y,2)));
end
fhandle = kernel.fhandle;
nMax = kernel.nMax;

y = [y, zeros(1, nMax)];  % add few more elements for computation convenience

% threshold of the spike
if ~exist('smin', 'var') || isempty(smin)
    smin = 3*sn;               % min spike count
else
    smin = smin * sn;
end

% fit response function or not
if ~exist('fit_gt', 'var') || isempty(fit_gt)
    fit_gt = false;
else
    kernel.pars = ar2exp(estimate_time_constant(y,2));
end
thresh = 1e-3;

% debug mode
if ~exist('debug_on', 'var') || isempty(debug_on)
    debug_on = false;
end

% maximum iterations for running OASIS
if ~exist('maxIter', 'var') || isempty(maxIter)
    maxIter = 5;
end

% tuning parameter for enforcing sparsity
if ~exist('lambda', 'var') || isempty(lambda)
    lambda = 0;   % tuning parameter
end

% weight for each frame
if ~exist('theta', 'var')|| isempty(theta) || (length(theta)==1)     % weight vector
    theta = ones(1, T);
elseif length(theta)==T
    theta = reshape(theta, 1, T);
else
    disp('elements in theta should be equal to elements in y');
    return;
end

% tuning parameter for enforcing sparsity
if ~exist('lambda', 'var') || isempty(lambda)
    lambda = 0;   % tuning parameter
end

%% running OASIS
if debug_on
    figure('position', [1, 1 1500, 250]);
    plot(y); hold on;
    a = plot(y, 'r');
    avi_file = VideoWriter('example.avi');
    avi_file.open();
    xlim([1, T]);
end

f0 = inf;       % objective function
y0 = y;         % backup the raw trace
iter = 1;       % iteratiosn
for iter = 1:maxIter
    if debug_on
        title(sprintf('Iter %d, norm(residual) = %.4f', iter, f0));
    end
    % normalize gt to make its maximum value to be 1
    gt = fhandle(kernel.pars, 1:nMax); % 1*nMax, response function of calcium transients
    ind0 = 1; %correct the response function of the first event
    gt = [reshape(gt, 1, nMax), zeros(1, T)]; % add few more elements to gt for computation convenience

    gt_max = max(gt);
    gt = gt/gt_max;
    gt1 = [gt(ind0:end), zeros(1, ind0-1)]; % kernel for the first frame

    %% initialize values for the results
    s = zeros(size(y));     % spike count within each bin
    v = zeros(size(y));     % sum(theta_t*y_t) - lambda
    w = zeros(size(y));     % sum(theta_t^2 * g(t-ti+1)^2))
    pool_ti = zeros(size(y)); % first frame of each pool

    % initialize the first pool
    v(1) = theta(1)*y(1)*gt1(1) - lambda;
    w(1) = (theta(1)*gt1(1))^2;
    s(1) = max(0, v(1) / w(1));
    %     if s(1)<smin
    %         s(1) = 0;
    %     end
    pool_ti(1) = 1;
    tpre = 1;   % time of the last event

    %% start to initialize the next pool
    ii = 2;     % start to initialize the next pool
    t = 2;      % frame to begin
    frame_valid = true;
    while t<=T
        % plot results
        if debug_on
            c = zeros(1,T);
            c(1:T) = s(1) * gt1(1:T);
            for m=2:(ii-1)
                t0 = pool_ti(m);
                c(t0:T) = c(t0:T)+s(t0) * gt(1:(T-t0+1));
            end
            delete(a);
            a = plot(c(1:t), 'r', 'linewidth', 2);
            drawnow;
            temp = getframe();
            temp.cdata = imresize(temp.cdata, [200, 1500]);
            avi_file.writeVideo(temp);
        end
        %
        % find the spike count that minimizes the objective function
        if (t-tpre)>=nMax  % no influences from the previous events
            vi = theta(t)*y(t)*gt(1) - lambda;
        elseif tpre>1        % estimate vi and subtract the effect of the previous event
            vi = theta(t) * (y(t)-s(tpre)*gt(t-tpre+1)) * gt(1) - lambda;
        else   % special treatment with the first event
            vi = theta(t) * (y(t)-s(tpre)*gt1(t-tpre+1)) * gt(1) - lambda;
        end
        wi = (theta(t) * gt(1))^2;
        si = vi/wi;

        % check the violatoin of si
        if si>smin && frame_valid % no violation, create a new pool to save the result and move to the next new pool
            % peel off the previous event
            if (t-tpre)<nMax
                if tpre>1
                    y(tpre+(1:nMax)-1) = y(tpre+(1:nMax)-1) - s(tpre)*gt(1:nMax);
                else
                    y(tpre+(1:nMax)-1) = y(tpre+(1:nMax)-1) - s(tpre)*gt1(1:nMax);
                end
            end
            pool_ti(ii) = t;   % create a new pool
            tpre = t;
            v(t) = vi;
            w(t) = wi;
            s(t) = si;
            ii = ii+1;  % move to the next pool
            t = t+1;
        else  % with violation
            frame_valid = true; % allows the next frame to be valid
            if (t-tpre)>=nMax
                % ignore this frame directly and move to the next frame
                t = t + 1;
                continue;
            elseif tpre>1         % merge it to the current pool
                v(tpre) = v(tpre) + theta(t)*y(t)*gt(t-tpre+1);
                w(tpre) = w(tpre) + (theta(t)*gt(t-tpre+1))^2;
            else
                v(tpre) = v(tpre) + theta(t)*y(t)*gt1(t-tpre+1);
                w(tpre) = w(tpre) + (theta(t)*gt1(t-tpre+1));
            end
            s(tpre) = v(tpre)/w(tpre);  % update the current event

            % check the violation of the current pool
            if s(tpre)>smin      % the previous event is still avaiable
                t = t+1;
                continue;
            elseif ii==2  %the previous pool is not available anymore, but it's the first pool
                s(tpre) = max(0, s(tpre));
                t = t+1;
            else   % not available, then delete the current pool and force its first frame to be
                % event-free.
                t = tpre;  % go back to the first frame of the current pull

                frame_valid = false; % force the frame t to be invalid
                ii = ii-1;
                tpre = pool_ti(ii-1);

                % add back the signal of the previous pool
                if (t-tpre)>nMax
                    t = t+1;
                    frame_valid = true;
                    continue;
                elseif tpre>1
                    y(tpre+(1:nMax)-1) = y(tpre+(1:nMax)-1) + s(tpre)*gt(1:nMax);
                else
                    y(tpre+(1:nMax)-1) = y(tpre+(1:nMax)-1) + s(tpre)*gt1(1:nMax);
                end
            end
        end
    end
    %% collect the results
    pool_ti(ii:end) = [];
    temp = s(pool_ti);
    s = zeros(1, T);
    s(pool_ti) = temp;
    temp = s;
    temp(1) = 0;
    c = conv(temp, gt(1:nMax));
    c = c(1:T) + s(1)*gt1(1:T);
    f1 = norm(y(1:T)-c, 2);
    s = s*sum(gt);
    if debug_on
        delete(a);
        a = plot(c, 'r');
        drawnow();
        temp = getframe();
        temp.cdata = imresize(temp.cdata, [200, 1500]);
        avi_file.writeVideo(temp);
    end

    %% break the while loop
    if ~fit_gt  % don't iterate gt
        break;
    elseif (f0-f1)/f0 <= thresh % improvement is small, stop
        break;
    else  % move to the next iteration
        y = y0;
        if strcmpi(kernel.type, 'exp2')
            [kernel, ~] = update_kernel_exp2(y(1:T), s, kernel);
        else
            break;
        end
        f0 = f1;
        iter = iter + 1;
        %             return;
    end
end

if debug_on
    avi_file.close();
    close(gcf);
end
end

%% estimate the noise power
function [sn,psdx,ff] = get_noise_fft(Y,options)
% Written by:
% Eftychios A. Pnevmatikakis, Simons Foundation, 2015
% with minor adaption by Pengcheng Zhou, Carnegie Mellon University, 2015
options.noise_range = [.25, .5];
range_ff = options.noise_range;
options.noise_method = 'logmexp';
method = options.noise_method;
options.block_size = [64, 64];
block_size = options.block_size;
options.split_data = false;
split_data = options.split_data;
options.max_timesteps = 3000;

dims = ndims(Y);
sizY = size(Y);
N = min(sizY(end),options.max_timesteps);
if N < sizY(end)
    %Y = reshape(Y,prod(sizY(1:end-1)),[]);
    Y(prod(sizY(1:end-1))*N+1:end) = [];
    Y = reshape(Y,[sizY(1:end-1),N]);
end

Fs = 1;
ff = 0:Fs/N:Fs/2;
indf=ff>range_ff(1);
indf(ff>range_ff(2))=0;
if dims > 1
    d = prod(sizY(1:dims-1));
    Y = reshape(Y,d,N);
    Nb = prod(block_size);
    SN = cell(ceil(d/Nb),1);
    PSDX = cell(ceil(d/Nb),1);
    if ~split_data
        for ind = 1:ceil(d/Nb);
            xdft = fft(Y((ind-1)*Nb+1:min(ind*Nb,d),:),[],2);
            xdft = xdft(:,1: floor(N/2)+1); % FN: floor added.
            psdx = (1/(Fs*N)) * abs(xdft).^2;
            psdx(:,2:end-1) = 2*psdx(:,2:end-1);
            %SN{ind} = mean_psd(psdx(:,indf),method);
            switch method
                case 'mean'
                    SN{ind}=sqrt(mean(psdx(:,indf)/2,2));
                case 'median'
                    SN{ind}=sqrt(median(psdx(:,indf)/2),2);
                case 'logmexp'
                    SN{ind} = sqrt(exp(mean(log(psdx(:,indf)/2),2)));
            end
            PSDX{ind} = psdx;
        end
    else
        nc = ceil(d/Nb);
        Yc = mat2cell(Y,[Nb*ones(nc-1,1);d-(nc-1)*Nb],N);
        parfor ind = 1:ceil(d/Nb);
            xdft = fft(Yc{ind},[],2);
            xdft = xdft(:,1:floor(N/2)+1);
            psdx = (1/(Fs*N)) * abs(xdft).^2;
            psdx(:,2:end-1) = 2*psdx(:,2:end-1);
            Yc{ind} = [];
            switch method
                case 'mean'
                    SN{ind}=sqrt(mean(psdx(:,indf)/2,2));
                case 'median'
                    SN{ind}=sqrt(median(psdx(:,indf)/2),2);
                case 'logmexp'
                    SN{ind} = sqrt(exp(mean(log(psdx(:,indf)/2),2)));
            end

        end
    end
    sn = cell2mat(SN);
else
    xdft = fft(Y);
    xdft = xdft(:,1:floor(N/2)+1);
    psdx = (1/(Fs*N)) * abs(xdft).^2;
    psdx(:,2:end-1) = 2*psdx(:,2:end-1);
    switch method
        case 'mean'
            sn = sqrt(mean(psdx(:,indf)/2,2));
        case 'median'
            sn = sqrt(median(psdx(:,indf)/2),2);
        case 'logmexp'
            sn = sqrt(exp(mean(log(psdx(:,indf)/2),2)));
    end
end
psdx = cell2mat(PSDX);
if dims > 2
    sn = reshape(sn,sizY(1:dims-1));
end
end


function nlp = truck_opt_cost(nlp, bounds, varargin)
    % add custom cost function
    sys = nlp.Plant;
    x = sys.States.x;
    dx = sys.States.dx;

    % 1. target states: 200*(x_f-x_des)'*P1*(x_f-x_des)
    rd = bounds.rd;
    x_des_val = sys.x_des(rd);
    x_f   = sys.States.x;
    x_d = SymVariable('xd',[8,1]);

    Q = diag([6 1 10 1 10 1]);
    R = 100;
    P1 = sys.pcare(Q,R);
    tar_pos = 100*(x_f-x_d).'*P1*(x_f-x_d);

    tar_pos_fn = SymFunction('target_pos',tar_pos,{x_f},{x_d});
    nlp = addNodeCost(nlp, tar_pos_fn, 'x', 'last', x_des_val');

    % 2. square intgeral of ddy and y
    ya = sys.VirtualConstraints.y.ActualFuncs;
    ya2 = ya{1};
    Dya2 = ya{3};
    ddya2 = Dya2*dx;

    yd2_square = SymFunction('yd2_square',tovector(500*ya2.^2),{x});
    nlp = addRunningCost(nlp, yd2_square, {'x'});

    ddyd2_square = SymFunction('ddyd2_square',tovector(0.1*ddya2.^2),{x,dx});
    nlp = addRunningCost(nlp, ddyd2_square, {'x','dx'});

    alpha_last = SymFunction('alpha_last',tovector(100*ya2.^2),{x});
    nlp = addNodeCost(nlp, alpha_last, {'x'},'last');

    % 6. norm(y) -> y[1]^2 + y[2]^2 + ... + y[n]^2 (remove the sqrt operator)
    y_squre = SymFunction('y_square',tovector(0.1*x(3).^2),{x});
    nlp = addNodeCost(nlp, y_squre, 'x', 'all');

    % 7. var(u) @todo how to compute the mean value

    % 8. max(abs(y))
    y_max = sys.Params.ymax;
    ymax_cost_fn = SymFunction('ymax_cost',tovector(20*y_max),{y_max});
    nlp = addNodeCost(nlp, ymax_cost_fn, 'ymax', 'first');
end


classdef SAMSO < ALGORITHM
% <single> <real/integer> <large/none> <expensive>
% Multiswarm-assisted expensive optimization

%------------------------------- Reference --------------------------------
% F. Li, X. Cai, L. Gao, and W. Shen, A surrogate-assisted multiswarm
% optimization algorithm for high-dimensional computationally expensive
% problems, IEEE Transactions on Cybernetics, 2021, 51(3): 1390-1402.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            assert(~isempty(ver('Optim')),'The execution of SAMPSO requires the Optimization Toolbox.');

            %% Parameter setting
            [Wnc,Pr] = Algorithm.ParameterSet(1,0.5);
            eta      = min(sqrt(0.001^2*Problem.D),5e-4*min(Problem.upper-Problem.lower));

            %% Initialze DB
            if Problem.D > 50
                N = 80;
                K = 2*Problem.D;
            else
                N = 40;
                K = N;
            end
            PopDec = UniformPoint(K,Problem.D,'Latin');
            Population = Problem.Evaluation(repmat(Problem.upper-Problem.lower,K,1).*PopDec+repmat(Problem.lower,K,1));

            %% Initialize swarm
            % Determine position
            [~,idx]  = sort(Population.objs,'ascend');
            Select   = idx(1:N);
            Position = [Population(Select).decs,Population(Select).objs];
            % velocity
            Vmax     = 0.5*(Problem.upper-Problem.lower);
            Vmin     = -0.5*Vmax;
            Velocity = rand(N,Problem.D).*(repmat(Vmax-Vmin,N,1)) + repmat(Vmin,N,1);
            % Pbest and Gbest
            Pbest    = Position;
            Gbest    = Position(1,:);
            maxFES   = Problem.maxFE - K;

            %% Optimization
            while Algorithm.NotTerminated(Population)
                % Build RBF surrogate model
                [model,~] = rbf_build(Population.decs,Population.objs);

                % Find the minimum of the surrogate
                srgtMin = FindOpt(model,Population,Problem.upper,Problem.lower);

                % Calculate distance
                dist  = pdist2(Population.decs,srgtMin);
                dxRBF = min(dist);
                if dxRBF > eta
                    optSrgt    = Problem.Evaluation(srgtMin);
                    Population = [Population,optSrgt];
                    if optSrgt.objs < Gbest(:,end)
                        [model,~] = rbf_build(Population.decs,Population.objs);
                        Gbest     = [optSrgt.decs,optSrgt.objs];
                    end
                end
                currFES = Problem.FE - K;
                [Population,Position,Velocity,Gbest,Pbest] = UpdatePosition(Problem,Population,Position,Velocity,Pbest,Gbest,currFES,maxFES,Wnc,Pr,model,eta);
            end
        end
    end
end


function logical_mask = build_logical_mask(nt, maxNt, Ns)

%   Copyright (C) 2009 Cesare Magri
%   Version: 1.0.4

% -------
% LICENSE
% -------
% This software is distributed free under the condition that:
%
% 1. it shall not be incorporated in software that is subsequently sold;
%
% 2. the authorship of the software shall be acknowledged and the following
%    article shall be properly cited in any publication that uses results
%    generated by the software:
%
%      Magri C, Whittingstall K, Singh V, Logothetis NK, Panzeri S: A
%      toolbox for the fast information analysis of multiple-site LFP, EEG
%      and spike train recordings. BMC Neuroscience 2009 10(1):81;
%
% 3.  this notice shall remain in place in each source file.

column_increment = 0:maxNt:(Ns*maxNt)-1;

mask = zeros(maxNt, Ns);
mask(column_increment + nt(:).') = 1;
mask(end:-1:1,:) = cumsum(mask(end:-1:1,:));
% Converting 1 to logical "true":
logical_mask = mask>0;


classdef nncrop_wrapper < nntest
  methods (Test)

    function basicShrink(test)
      batchSize = 10 ;
      x1 = test.randn([10 10 3 batchSize]) ;
      x2 = test.randn([5 5 3 batchSize]) ;
      crop = [1 1] ;
      y = vl_nncrop_wrapper(x1, x2, crop) ;

      % check derivatives with numerical approximation
      dzdy = test.randn(size(y)) ;
      dzdx1 = vl_nncrop_wrapper(x1, x2, crop, dzdy) ;
      test.der(@(x1) vl_nncrop_wrapper(x1, x2, crop), ...
                               x1, dzdy, dzdx1, 1e-3*test.range) ;
    end
  end
end


function test_issue856

% MEM 3gb
% WALLTIME 00:10:00
% DEPENDENCY ft_determine_coordsys ft_convert_coordsys ft_determine_units ft_convert_units ft_transform_geometry

%%

mri0 = ft_read_mri(dccnpath('/home/common/matlab/fieldtrip/data/ftp/test/ctf/Subject01.mri'));
grad0 = ft_read_sens(dccnpath('/home/common/matlab/fieldtrip/data/ftp/test/ctf/Subject01.ds'), 'senstype', 'meg');
headmodel0 = ft_read_headmodel(dccnpath('/home/common/matlab/fieldtrip/data/ftp/test/ctf/Subject01.hdm'));

ft_hastoolbox('spm12', 1);

%%
% these have mixed units

mri1 = ft_determine_units(mri0);
grad1 = ft_determine_units(grad0);
headmodel1 = ft_determine_units(headmodel0);

%%
% convert to meter

target = 'm';
mri2 = ft_convert_units(mri1, target);
grad2 = ft_convert_units(grad1, target);
headmodel2 = ft_convert_units(headmodel1, target);

assert(~isequal(mri1.transform, mri2.transform));
assert(~isequal(grad1.coilpos, grad2.coilpos));
assert(~isequal(headmodel1.r, headmodel2.r));

%%
% convert to acpc

method = 0; % only approximate
mri3 = ft_convert_coordsys(mri2, 'acpc', method);
grad3 = ft_convert_coordsys(grad2, 'acpc', method);
headmodel3 = ft_convert_coordsys(headmodel2, 'acpc', method);

%%
% convert to millimeter

target = 'mm';
mri2 = ft_convert_units(mri1, target);
grad2 = ft_convert_units(grad1, target);
headmodel2 = ft_convert_units(headmodel1, target);

%%
% spm_affreg
mri4 = ft_convert_coordsys(mri2, 'acpc', 1);

% this round-trip is not exactly identical
mri4b = ft_convert_coordsys(mri4, 'ctf', 0);
assert(~isequal(mri2.transform, mri4b.transform));

%%
% smp_normalise
mri5 = ft_convert_coordsys(mri2, 'acpc', 2);

% this round-trip is not exactly identical
mri5b = ft_convert_coordsys(mri5, 'ctf', 0);
assert(~isequal(mri2.transform, mri5b.transform));


function [ci,bootstat]  = stat_bootci(nboot,bootfun,varargin)
%BOOTCI Bootstrap Confidence Interval
%   CI = BOOTCI(NBOOT,BOOTFUN,...) computes the 95 percent BCa bootstrap
%   confidence interval of the statistic defined by the function BOOTFUN.
%   NBOOT is a positive integer indicating the number of bootstrap data
%   samples used in the computation. BOOTFUN is a function handle specified
%   with @. The third and later input arguments to BOOTCI are data
%   (scalars, column vectors, or matrices) that are used to create inputs
%   to BOOTFUN. BOOTCI creates each bootstrap sample by sampling with
%   replacement from the rows of the non-scalar data arguments (these must
%   have the same number of rows). Scalar data are passed to BOOTFUN
%   unchanged.
%
%   If BOOTFUN returns a scalar, CI is a vector containing the lower and
%   upper bounds of the confidence interval. If BOOTFUN returns a vector of
%   length M, CI is an array of size 2-by-M, where CI(1,:) are lower bounds
%   and CI(2,:) are upper bounds. If BOOTFUN returns an array of size
%   M-by-N-by-P-by-..., CI is an array of size 2-by-M-by-N-by-P-by-...,
%   where CI(1,:,:,:,...) is an array of lower bounds and CI(2,:,:,:,...)
%   is an array of upper bounds.
%
%   CI = BOOTCI(NBOOT,{BOOTFUN,...},'alpha',ALPHA) computes the 100*(1-ALPHA)
%   percent BCa bootstrap confidence interval of the statistic defined by the
%   function BOOTFUN. ALPHA is a scalar between 0 and 1. The default value of
%   ALPHA is 0.05. A cell array groups BOOTFUN and the arguments used to create
%   inputs to it. ALPHA and any other arguments to BOOTCI appear outside the
%   cell array.
%
%   CI = BOOTCI(NBOOT,{BOOTFUN,...},...,'type',TYPE) computes the bootstrap
%   confidence interval of the statistic defined by the function BOOTFUN.
%   TYPE is the confidence interval type, specifying different methods of
%   computing the confidence interval. TYPE is a string chosen from
%       'norm' or 'normal':               normal approximated interval with
%                                         bootstrapped bias and standard
%                                         error;
%       'per' or 'percentile':            basic percentile method;
%       'cper' or 'corrected percentile': bias corrected percentile method;
%       'bca' :                           bias corrected and accelerated
%                                         percentile method;
%       'stud' or 'student':              studentized confidence interval.
%   The default value of TYPE is 'bca'.
%
%   CI = BOOTCI(NBOOT,{BOOTFUN,...},...,'type','stud','nbootstd',NBOOTSTD)
%   computes the studentized bootstrap confidence interval of the statistic
%   defined by the function BOOTFUN. The standard error of the bootstrap
%   statistics is estimated using bootstrap with NBOOTSTD bootstrap data
%   samples. NBOOTSTD is a positive integer value. The default value of
%   NBOOTSTD is 100.
%
%   CI = BOOTCI(NBOOT,{BOOTFUN,...},...,'type','stud','stderr',STDERR)
%   computes the studentized bootstrap confidence interval of statistics
%   defined by the function BOOTFUN. The standard error of the bootstrap
%   statistics is evaluated by the function STDERR. STDERR is a function
%   handle created using @. STDERR should take the same arguments as
%   BOOTFUN and return the standard error of the statistic computed by
%   BOOTFUN.
%
%   CI = BOOTCI(NBOOT,{BOOTFUN,...},...,'Weights',WEIGHTS) specifies
%   observation weights. WEIGHTS must be a vector of non-negative numbers
%   with at least one positive element. The number of elements in WEIGHTS
%   must be equal to the number of rows in non-scalar input arguments to
%   BOOTFUN. To obtain one bootstrap replicate, BOOTSTRP samples N out of N
%   with replacement using these weights as multinomial sampling
%   probabilities.
%
%   CI = BOOTCI(NBOOT,{BOOTFUN,...},...,'Options',OPTIONS) contains options
%   that specify whether to compute bootstrap iterations in parallel, and how
%   to use random numbers during the bootstrap sampling. OPTIONS is a struct
%   that can be created by a call to STATSET. BOOTCI uses the following fields:
%       'UseParallel'
%       'UseSubstreams'
%       'Streams'
%   For information on these fields see PARALLELSTATS.
%   NOTE: if 'UseParallel' is 'always' and 'UseSubstreams'
%   is 'never', then the length of Streams must equal the number
%   of processors used by BOOTCI. There are two possibilities.
%   If a MATLAB pool is open, then Streams is the same length as
%   the size of the MATLAB pool. If a MATLAB pool is not open,
%   then Streams must supply a single random number stream.
%
%   [CI,BOOTSTAT] = BOOTCI(...) also returns the bootstrapped statistic
%   computed for each of the NBOOT bootstrap replicate samples.  Each row of
%   BOOTSTAT contains the results of applying BOOTFUN to one bootstrap sample.
%   If BOOTFUN returns a matrix or array, then this output is converted to a
%   row vector for storage in BOOTSTAT.
%
%   Example:
%     Compute the confidence interval for the capability index in
%     statistical process control:
%          y = normrnd(1,1,30,1);                  % simulated process data
%          LSL = -3;  USL = 3;                     % process specifications
%          capable = @(x) (USL-LSL)./(6* std(x));  % process capability
%          bootci(2000,capable, y)                 % Bca confidence interval
%          bootci(2000,{capable, y},'type','per')  % basic percentile method
%
%   See also: BOOTSTRP, JACKKNIFE, STATSET, STATGET,
%   RANDSAMPLE, PARFOR, PARALLELSTATS.

% The BCa method is described in the following references:
%
%     T.J. DiCicio and B. Efron (1996), "Bootstrap confidence intervals,"
%     Statistical Science, v. 11, n. 3, pp. 189-228.
%
%     B. Efron and R.J. Tibshirani (1993), An Introduction to the
%     Bootstrap, Chapman & Hall, New York.
%
% Their formula involves a Z0 factor that is computed using the proportion
% of bootstrap values less than the original sample value.  In order to get
% reasonable results when the sample is lumpy, we include half of the
% bootstrap values that are tied with the original sample value when we
% compute Z0.

% Copyright 2005-2010 The MathWorks, Inc.
%   $Revision: 1.1.8.8 $  $Date: 2011/07/20 00:08:12 $

if nargin<2
    error(message('stats:bootci:TooFewInputs'));
end;
if nboot<=0 || nboot~=round(nboot)
    error(message('stats:bootci:BadNboot'))
end;

if ~iscell(bootfun) % default syntax
    type = 'bca';
    alpha = .05;
    fun = bootfun;
    data = varargin;
    weights = [];
    bootstrpOptions = statset('bootstrp');
else % syntax with optional type, alpha, nbootstd, and stderrfun name/value pairs
    fun = bootfun{1};
    data = bootfun(2:end);
    pnames = {'type', 'alpha', 'stderr', 'nbootstd', 'weights', 'options'};
    dflts =  {'bca', .05, [], 100, [], statset('bootstrp')};
    [type,alpha,stderrfun,nbootstd,weights,bootstrpOptions] = ...
                           internal.stats.parseArgs(pnames, dflts, varargin{:});
end

% error check for the bootfun
try
    obsstat = fun(data{:});
catch ME
    m = message('stats:bootci:BadBootfun',func2str(fun));
    throw(addCause(MException(m.Identifier,'%s',getString(m)),ME));
end
if any(~isfinite(obsstat))
        error(message('stats:bootci:NonfiniteBootfun'));
end;

% Save the original size of stat and reshape it
sz = [];
if ~isvector(obsstat)
    sz = size(obsstat);
end
obsstat = obsstat(:)'; % turn into a row-vector

% call subfunctions to compute the intervals
switch (lower(type))
    case {'norm','normal'}
        [ci,bootstat] = bootnorm(obsstat,nboot,fun,alpha,weights,bootstrpOptions,data{:});
    case {'per','percentile'}
        [ci,bootstat] = bootper(nboot,fun,alpha,weights,bootstrpOptions,data{:});
    case {'cper', 'corrected percentile'}
        [ci,bootstat] = bootcper(obsstat,nboot,fun,alpha,weights,bootstrpOptions,data{:});
    case 'bca'
        [ci,bootstat] = bootbca(obsstat,nboot,fun,alpha,weights,bootstrpOptions,data{:});
    case {'stud','student'}
        [ci,bootstat] = bootstud(obsstat,nboot,fun,alpha,nbootstd,stderrfun,...
            weights,bootstrpOptions,data{:});
    otherwise
        error(message('stats:bootci:BadType'))
end;

% Reshape
if ~isempty(sz)
    ci = reshape(ci,[2 sz]);
end

end   % bootci()

%-------------------------------------------------------------------------
function [ci,bstat] = bootnorm(stat,nboot,bootfun,alpha,weights,bootstrpOptions,varargin)
% normal approximation interval
% A.C. Davison and D.V. Hinkley (1996), p198-200

bstat = bootstrp(nboot,bootfun,varargin{:},'weights',weights,'Options',bootstrpOptions);

se = std(bstat,0,1);   % standard deviation estimate
bias = mean(bsxfun(@minus,bstat,stat),1);
za = norminv(alpha/2);   % normal confidence point
lower = stat - bias + se*za; % lower bound
upper = stat - bias - se*za;  % upper bound

% return
ci = [lower;upper];
end   % bootnorm()

%-------------------------------------------------------------------------
function [ci,bstat] = bootper(nboot,bootfun,alpha,weights,bootstrpOptions,varargin)
% percentile bootstrap CI

bstat = bootstrp(nboot,bootfun,varargin{:},'weights',weights,'Options',bootstrpOptions);

pct1 = 100*alpha/2;
pct2 = 100-pct1;
lower = prctile(bstat,pct1,1);
upper = prctile(bstat,pct2,1);

% return
ci =[lower;upper];
end % bootper()

%-------------------------------------------------------------------------
function [ci,bstat] = bootcper(stat,nboot,bootfun,alpha,weights,bootstrpOptions,varargin)
% corrected percentile bootstrap CI
% B. Efron (1982), "The jackknife, the bootstrap and other resampling
% plans", SIAM.

bstat = bootstrp(nboot,bootfun,varargin{:},'weights',weights,'Options',bootstrpOptions);

% stat is transformed to a normal random variable z0.
% z0 = invnormCDF[ECDF(stat)]
z_0 = fz0(bstat,stat);
z_alpha = norminv(alpha/2); % normal confidence point

% transform z0 back using the invECDF[normCDF(2z0-za)] and
% invECDF[normCDF(2z0+za)]
pct1 = 100*normcdf(2*z_0-z_alpha);
pct2 = 100*normcdf(2*z_0+z_alpha);

% inverse ECDF
m = numel(stat);
lower = zeros(1,m);
upper = zeros(1,m);
for i=1:m
    lower(i) = prctile(bstat(:,i),pct2(i),1);
    upper(i) = prctile(bstat(:,i),pct1(i),1);
end

% return
ci = [lower;upper];
end % bootcper()

%-------------------------------------------------------------------------
function [ci,bstat] = bootbca(stat,nboot,bootfun,alpha,weights,bootstrpOptions,varargin)
% corrected and accelerated percentile bootstrap CI
% T.J. DiCiccio and B. Efron (1996), "Bootstrap Confidence Intervals",
% statistical science, 11(3)

bstat = bootstrp(nboot,bootfun,varargin{:},'weights',weights,'Options',bootstrpOptions);

% same as bootcper, this is the bias correction
z_0 = fz0(bstat,stat);

% apply jackknife
try
    jstat = jackknife(bootfun,varargin{:},'Options',bootstrpOptions);
catch ME
    m = message('stats:bootci:JackknifeFailed',func2str(bootfun));
    throw(addCause(MException(m.Identifier,'%s',getString(m)),ME));
end
N = size(jstat,1);
if isempty(weights)
    weights = repmat(1/N,N,1);
else
    weights = weights(:);
    weights = weights/sum(weights);
end

% acceleration finding, see DiCiccio and Efron (1996)
mjstat = sum(bsxfun(@times,jstat,weights),1); % mean along 1st dim.
score = bsxfun(@minus,mjstat,jstat); % score function at stat; ignore (N-1) factor because it cancels out in the skew
iszer = all(score==0,1);
skew = sum(bsxfun(@times,score.^3,weights),1) ./ ...
    (sum(bsxfun(@times,score.^2,weights),1).^1.5) /sqrt(N); % skewness of the score function
skew(iszer) = 0;
acc = skew/6;  % acceleration

% transform back with bias corrected and acceleration
z_alpha1 = norminv(alpha/2);
z_alpha2 = -z_alpha1;
pct1 = 100*normcdf(z_0 +(z_0+z_alpha1)./(1-acc.*(z_0+z_alpha1)));
pct1(z_0==Inf) = 100;
pct1(z_0==-Inf) = 0;
pct2 = 100*normcdf(z_0 +(z_0+z_alpha2)./(1-acc.*(z_0+z_alpha2)));
pct2(z_0==Inf) = 100;
pct2(z_0==-Inf) = 0;

% inverse of ECDF
m = numel(stat);
lower = zeros(1,m);
upper = zeros(1,m);
for i=1:m
    lower(i) = prctile(bstat(:,i),pct2(i),1);
    upper(i) = prctile(bstat(:,i),pct1(i),1);
end

% return
ci = sort([lower;upper],1);
end % bootbca()

%-------------------------------------------------------------------------
function [ci,bstat] = bootstud(stat,nboot,bootfun,alpha,nbootstd,stderrfun,weights,bootstrpOptions,varargin)
% studentized bootstrap CI with bootstrp to estimate the se
% T.J. DiCiccio and B. Efron (1996), "Bootstrap Confidence Intervals",
% statistical science, 11(3)

% Should we use stderrfun to compute st. dev. or an inner bootstrap loop?
if isempty(stderrfun) % studentized with bootstrap error
    if nbootstd<=0 || nbootstd~=round(nbootstd)
        error(message('stats:bootci:BadNbootstd'))
    end
else  % studentized with stderrfun fun
    % error check for stderrfun
    try
        out=stderrfun(varargin{:});
    catch ME
        m = message('stats:bootci:BadStderr',func2str(stderrfun));
        throw(addCause(MException(m.Identifier,'%s',getString(m)),ME));
    end
    if any(~isfinite(out))
        error(message('stats:bootci:NonfiniteStderr'));
    end
    if isvector(out)
        out = out(:)';
    end
    if size(stat)~=size(out)
        error(message('stats:bootci:BadStderrSize'));
    end
end;

% bootstrap
[bstat,bootsam] = bootstrp(nboot,bootfun,varargin{:}, ...
    'weights',weights,'Options',bootstrpOptions); % bootstrap statistics

% find non-scalar data in varargin
la = length(varargin);
scalard = zeros(la,1);
for k = 1:la
   [row,col] = size(varargin{k});
   if max(row,col) == 1
      scalard(k) = 1;
   end
   if row == 1 && col ~= 1
      varargin{k} = varargin{k}(:);
   end
end

% bootstrap generated bootstrap replica to get student errors
N = size(bootsam,1);
sd_t = zeros(nboot,numel(stat));
for b=1:nboot
    db = cell(la,1);
    for k = 1:la
        % store the bootstrap data samples in a cell array
        if scalard(k) == 0
            db{k} = varargin{k}(bootsam(:,b),:);
        else
            db{k} = varargin{k};
        end
    end
    if isempty(weights)
        w = [];
    else
        w = weights(bootsam(:,b));
    end
    if isempty(stderrfun)
        bstatstd = bootstrp(nbootstd,bootfun,db{:},'weights',w,...
            'Options',bootstrpOptions);
        sd_t(b,:) = std(bstatstd,0,1);
    else
        bstatstd = stderrfun(db{:});
        sd_t(b,:) = reshape(bstatstd,1,numel(stat));
    end
end

% studentized statistics
above0 = sd_t>0;
mbstat = bsxfun(@minus,bstat,stat);
tstat = zeros(size(mbstat));
tstat(above0) = mbstat(above0)./sd_t(above0);

% percentiles for the studentized stats are computed.
lower = prctile(tstat,100*alpha/2,1);
upper = prctile(tstat,100*(1-alpha/2),1);

% back to the original stats from the studentized stats
lower = lower.*std(bstat,0,1) + stat;
upper = upper.*std(bstat,0,1) + stat;

% return
ci = [lower; upper];
end

% -------------------------
function z0=fz0(bstat,stat)
% Compute bias-correction constant z0
z0 = norminv(mean(bsxfun(@lt,bstat,stat),1) + mean(bsxfun(@eq,bstat,stat),1)/2);
end   % fz0()


function [S,f,R,Serr]=mtspectrumtrigpt(data,E,win,params,fscorr)
% Multi-taper time-frequency spectrum - point process times
%
% Usage:
%
% [S,f,R,Serr]=mtspectrumtrigpt(data,E,win,params,fscorr)
% Input:
%       data        (structure array of one channel of spike times;
%                   also accepts 1d column vector of spike times) -- required
%       E           (event times) - required
%       win         (in the form [winl winr] i.e window around each event)--
%                                                 required
%       params: structure with fields tapers, pad, Fs, fpass, err, trialave
%       - optional
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                   (1) A numeric vector [TW K] where TW is the
%                       time-bandwidth product and K is the number of
%                       tapers to be used (less than or equal to
%                       2TW-1).
%                   (2) A numeric vector [W T p] where W is the
%                       bandwidth, T is the duration of the data and p
%                       is an integer such that 2TW-p tapers are used. In
%                       this form there is no default i.e. to specify
%                       the bandwidth, you have to specify T and p as
%                       well. Note that the units of W and T have to be
%                       consistent: if W is in Hz, T must be in seconds
%                       and vice versa. Note that these units must also
%                       be consistent with the units of params.Fs: W can
%                       be in Hz if and only if params.Fs is in Hz.
%                       The default is to use form 1 with TW=3 and K=5
%
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%           Fs   (sampling frequency) - optional. Default 1.
%           fpass    (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%           err  (error calculation [1 p] - Theoretical error bars; [2 p] - Jackknife error bars
%                                   [0 p] or 0 - no error bars) - optional. Default 0.
%           trialave (average over events when 1, don't average when 0) -
%           default 0
%       fscorr   (finite size corrections, 0 (don't use finite size corrections) or
%                1 (use finite size corrections) - optional
%                (available only for spikes). Defaults 0.
%
% Output:
%       S       (triggered spectrum in form frequency x events if trialave=0;
%               function of frequency if trialave=1)
%       f       (frequencies)
%       R       (spike rate)
%       Serr    (error bars) - only for err(1)>=1

if nargin < 3; error('Need data, events and window parameters'); end;
if nargin < 2; params=[]; end;
[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
clear tapers pad Fs fpass trialave
if nargin < 5 || isempty(fscorr); fscorr=0; end;
if nargout > 3 && err(1)==0; error('Cannot compute errors if err(1)=0'); end;
data=createdatamatpt(data,E,win);
if nargout==4;
    [S,f,R,Serr]=mtspectrumpt(data,params,fscorr);
else
    [S,f,R]=mtspectrumpt(data,params,fscorr);
end;


function [fBetaBstMean, fBetaBstStd, mResult] = calc_beta_bootstrap(mCat,nBst,nSample)
% function [fBetaBstMean, fBetaBstStd, mResult] = calc_beta_bootstrap(mCat,nBst,nSample)
% -------------------------------------------------------------------------
%
% Calculate beta uncertainty for Michaels` Stress Tensor inversion approach boostrapping
% the Focal Mechanisms of the earthquake catalog mCat. This is a stability
% test of the method. No flipping of focal mechanisms included.
%
% Incoming
% mCat: EQ catalog or [dip direction dip rake]
% nBst: Number of boostraps
% nSample: Number of events from catalog to be used, default is all
%

if ~exist('nSample','var')
    nSample = length(mCat(:,1));
end

% Bootstrap event to be used
[nNy,nNx] = size(mCat);
if nNx > 3
    mCat = mCat(:,10:12);
end

% Bootstrap numbers
vNy = 1:1:nNy;
mBootstrap = bootrsp(vNy,nBst);

%Path
sZmapPath = './AddOneFiles/zmap/';

% Container for bootstrap beta values
mResult = [];

for nCnt = 1:1:nBst
    mCatBst = mCat(mBootstrap(1:nSample,nCnt),:);
    % Array of focal mechanisms: dip direction, dip, rake
    mFPS = mCatBst;

    % Do inversion using A. Michael code
    % Create file for inversion
    fid = fopen('data2','w');
    str = ['Inversion data'];str = str';
    fprintf(fid,'%s  \n',str');
    fprintf(fid,'%7.3f  %7.3f  %7.3f\n',mFPS');
    fclose(fid);
    % slick calculates the best solution for the stress tensor according to
    % Michael(1987): creates data2.oput
    sPath = pwd;
    unix([sZmapPath 'external/slick ' sPath '/data2 ']);

    % Get data from data2.oput
    sFilename = ['data2.oput'];
    % Calculate average angle between tangential traction predicted by best
    % stress tensor and the slip direction
    [fBeta, fStdBeta, fTauFit, fAvgTau, fStdTau] = import_slickoput(sFilename);

    % Delete existing from earlier runs data2.slboot
    sData2 = ['data2.slboot'];
    delete(sData2);

    % Stress tensor inversion
    unix([sZmapPath 'external/slfast ' sPath '/data2 ']);
    sGetFile = ['data2.slboot'];
    load(sGetFile);
    % Description of data2
    % Line 1: Variance S11 S12 S13 S22 S23 S33 => Variance and components of
    % stress tensor (S = sigma)
    % Line 2: Phi S1t S1p S2t S2p S3t S3p
    % Phi is relative size S2/S1, t=trend, p=plunge (other description)
    fVariance = data2(1,1);
%     fS11 = data2(1,2);
%     fS12 = data2(1,3);
%     fS13 = data2(1,4);
%     fS22 = data2(1,5);
%     fS23 = data2(1,6);
%     fS33 = data2(1,7);
    fPhi = data2(2,1);
    fS1Trend = data2(2,2);
    fS1Plunge = data2(2,3);
    fS2Trend = data2(2,4);
    fS2Plunge = data2(2,5);
    fS3Trend = data2(2,6);
    fS3Plunge = data2(2,7);
    % Container
    mResult = [mResult; fBeta fStdBeta fTauFit fAvgTau fStdTau fVariance fPhi fS1Trend fS1Plunge fS2Trend fS3Trend fS3Plunge];
end

% Bootstrap standar deviation
fBetaBstStd = calc_StdDev(mResult(:,1));
fBetaBstMean = mean(mResult(:,1));


function isgn = r8vec_compare ( n, a1, a2 )

%*****************************************************************************80
%
%% R8VEC_COMPARE compares two R8VEC's.
%
%  Discussion:
%
%    The lexicographic ordering is used.
%
%  Example:
%
%    Input:
%
%      A1 = ( 2.0, 6.0, 2.0 )
%      A2 = ( 2.0, 8.0, 12.0 )
%
%    Output:
%
%      ISGN = -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vectors.
%
%    Input, real A1(N), A2(N), the vectors to be compared.
%
%    Output, integer ISGN, the results of the comparison:
%    -1, A1 < A2,
%     0, A1 = A2,
%    +1, A1 > A2.
%
  isgn = 0;

  k = 1;

  while ( k <= n )

    if ( a1(k) < a2(k) )
      isgn = -1;
      return
    elseif ( a2(k) < a1(k) )
      isgn = +1;
      return
    end

    k = k + 1;

  end

  return
end


function [out_val,out_ind,nevals]=tt_stat(tt, varargin)
% Compute statistics (min,max) of a tt_tensor via maxvol cross
%   [out_val,out_ind,nevals]=tt_stat(tt, varargin)
%
% Tries to compute values and indices of the sought quantities, listed in
% varargin. Possible choices are (case insensitive):
%   'LM', 'SM': largest, resp. smallest in magnitude entries,
%   'LR', 'SR': entries having largest, resp. smallest real parts, or
%   'LI', 'SI': entries having largest, resp. smallest imaginary parts.
% out_val is a 1 x N vector of output values, listed in the same order as
% in varargin, out_ind is a d x N matrix of indices, where d is the
% dimension of tt. The number of inspected elements is returned in nevals.
%
% Additional parameters can be given in varargin in the form
% 'param_name', param_value:
%       'kickrank': enrich tt by random subspaces of sizes kickrank. Larger
%       kickrank may improve the accuracy in "difficult" cases, for the price
%       of larger neval. Default value is 0.
%       'nswp': number of alternating iterations (default 3)
%
% Examples:
%   Mimic the call to tt_abs_max, tt_max and tt_min simultaneously:
%       [out,ind,neval]=tt_stat(tt, 'lm', 'lr', 'sr');
%   The same with kickrank set to 5:
%       [out,ind,neval]=tt_stat(tt, 'lm', 'lr', 'sr', 'kickrank', 5);
%   Minimize the banana function on a grid 32768 x 32768:
%       n = 15;
%       x = tkron(tt_x(2,n)/2^n, tt_ones(2,n));
%       y = tkron(tt_ones(2,n), tt_x(2,n)/2^n);
%       f = (0.5-x).^2 + 100*(y-x.^2).^2;
%       v = tt_stat(f,'sm','nswp',11)
%   This should give v ~ 2e-13.
%
% The difference with tt_*max* is the usage of dnr^2 entries
% instead of dr^2 (outputs are sought over TT blocks, not density matrices).
% Besides, it can carry out several iterations with random enrichment.

kickrank = 0; % random enrich may actually perturb the values, but sometimes it converges faster
nswp = 3; % these seem to be enough quite often

if (numel(varargin)==0)
    error('at least one sought quantity is needed');
end;

% Distinguish soughts from parameters
soughts = cell(1,numel(varargin));
j = 1;
while (j<=numel(varargin))
    if    ((strcmpi(varargin{j}, 'lm'))||(strcmpi(varargin{j}, 'sm'))||...
           (strcmpi(varargin{j}, 'lr'))||(strcmpi(varargin{j}, 'sr'))||...
           (strcmpi(varargin{j}, 'li'))||(strcmpi(varargin{j}, 'si')))
       soughts{j} = varargin{j};
    end;
    if (strcmpi(varargin{j}, 'kickrank'))
        kickrank = varargin{j+1};
        j = j+1;
    end;
    if (strcmpi(varargin{j}, 'nswp'))
        nswp = varargin{j+1};
        j = j+1;
    end;
    j = j+1;
end;
soughts(cellfun('isempty', soughts))=[];

d = tt.d;
n = tt.n;
r = tt.r;
tt = core2cell(tt);
r0 = r; % r will become different below

ind_global = cell(d+1,1);
ind_local = cell(d+1,1); % for reducing units
out_val = nan*ones(1,numel(soughts));
out_ind = zeros(d,numel(soughts));
% reductions
XX0 = cell(d+1,1);
XX0{1} = 1; XX0{d+1} = 1;

% We need to subtract unitary vectors to exlude already considered indices
% and select fresh ones
% Prepare storages for them
units = cell(d,numel(soughts));
for j=1:numel(soughts)
    for i=1:d
        units{i,j} = zeros(n(i),1);
    end;
end;
XU = cell(d+1,numel(soughts));
for j=1:numel(soughts)
    XU{1,j}=1; XU{d+1,j}=1;
end;
nevals = 0;

% Start iteration
swp = 0; % Zeroth iteration will be just QR and MV
dir = -1;
i = d;
while (swp<=nswp)
    if (swp>0)
        % Compute statistics
        q = reshape(tt{i}, r0(i), n(i)*r0(i+1));
        q = XX0{i}*q;
        q = reshape(q, r(i)*n(i), r0(i+1));
        q = q*XX0{i+1};
        q = reshape(q, r(i)*n(i)*r(i+1), 1);
        nevals = nevals + r(i)*n(i)*r(i+1); % number of evaluations
        ind_new = zeros(numel(soughts), 3);
        % We need to add the found extrema indices to maxvol, since they
        % will likely give good estimates in the next dimensions.
        pos = 0;
        for j=1:numel(soughts)
            out_changed = false;
            if (strcmpi(soughts{j}, 'lm'))
                [val,ind]=max(abs(q));
                out_changed = (val>abs(out_val(j)))||(isnan(out_val(j)));
            end;
            if (strcmpi(soughts{j}, 'lr'))
                [val,ind]=max(real(q));
                out_changed = (val>real(out_val(j)))||(isnan(out_val(j)));
            end;
            if (strcmpi(soughts{j}, 'li'))
                [val,ind]=max(imag(q));
                out_changed = (val>imag(out_val(j)))||(isnan(out_val(j)));
            end;
            if (strcmpi(soughts{j}, 'sm'))
                [val,ind]=min(abs(q));
                out_changed = (val<abs(out_val(j)))||(isnan(out_val(j)));
            end;
            if (strcmpi(soughts{j}, 'sr'))
                [val,ind]=min(real(q));
                out_changed = (val<real(out_val(j)))||(isnan(out_val(j)));
            end;
            if (strcmpi(soughts{j}, 'si'))
                [val,ind]=min(imag(q));
                out_changed = (val<imag(out_val(j)))||(isnan(out_val(j)));
            end;
            if (out_changed)
                out_val(j) = q(ind);
                ind = tt_ind2sub([r(i), n(i), r(i+1)], ind);
                % Add the new index to the expansion set
                ind_new(pos+1,:) = ind;
                pos = pos+1;
                if (i>1)&&(i<d)
                    out_ind(:,j) = indexmerge(ind_global{i}(ind(1),:), ind(2), ind_global{i+1}(ind(3),:));
                elseif (i==1)
                    out_ind(:,j) = indexmerge(ind(2), ind_global{i+1}(ind(3),:));
                else % i==d
                    out_ind(:,j) = indexmerge(ind_global{i}(ind(1),:), ind(2));
                end;
                % Update units vectors
                for k=1:d
                    units{k,j}=zeros(n(k),1);
                    units{k,j}(out_ind(k,j))=1;
                end;
                % and their reductions
                for k=1:i-1
                    XU{k+1,j} = XU{k,j}*units{k,j}.';
                    XU{k+1,j} = reshape(XU{k+1,j}, r(k)*n(k), 1);
                    XU{k+1,j} = XU{k+1,j}(ind_local{k+1}, :);
                end;
                for k=d:-1:i+1
                    XU{k,j} = units{k,j}*XU{k+1,j};
                    XU{k,j} = reshape(XU{k,j}, 1, n(k)*r(k+1));
                    XU{k,j} = XU{k,j}(:, ind_local{k});
                end;
            end;
        end;
        ind_new = ind_new(1:pos, :);

        % Subtract units to get fresh indices
        for j=1:numel(soughts)
            qu = XU{i,j}*reshape(units{i,j}*XU{i+1,j}, 1, n(i)*r(i+1));
            qu = reshape(qu, r(i)*n(i)*r(i+1),1);
            q = q-qu*out_val(j);
        end;
    end;

    if (dir>0)&&(i<d)
        q = reshape(q, r(i)*n(i), r(i+1));
        rr = zeros(r(i)*n(i), pos);
        % Enrich it by the extrema indices (just add corresponding unit
        % vectors)
        if (pos>0)
            for j=1:pos
                rr(tt_sub2ind([r(i), n(i)], ind_new(j,1:2)), j) = 1;
            end;
        end;
        % Enrich it by random
        rr2 = randn(r(i)*n(i), kickrank);
        [q,~]=qr([q, rr, rr2],0);
        rnew = size(q,2);

        % Maxvol
        ind = maxvol2(q);
        qq = q(ind,:);
        q = q/qq;
        % Indices
        ind_local{i+1} = ind; % for units
        ind_global{i+1} = indexmerge(ind_global{i}, (1:n(i))');
        ind_global{i+1} = ind_global{i+1}(ind,:);
        r(i+1) = rnew;
        % Reduction
        XX0{i+1} = reshape(tt{i}, r0(i), n(i)*r0(i+1));
        XX0{i+1} = XX0{i}*XX0{i+1};
        XX0{i+1} = reshape(XX0{i+1}, r(i)*n(i), r0(i+1));
        XX0{i+1} = XX0{i+1}(ind,:);
        % For units
        for j=1:numel(soughts)
            XU{i+1,j} = XU{i,j}*reshape(units{i,j}, 1, n(i));
            XU{i+1,j} = reshape(XU{i+1,j}, r(i)*n(i), 1);
            XU{i+1,j} = XU{i+1,j}(ind, :);
        end;
    elseif (dir<0)&&(i>1)
        % QR and right indices
        if (swp==0)
            % Don't perturb the original solution, just QR
            q = reshape(tt{i}, r(i), n(i)*r(i+1));
            rr = zeros(n(i)*r(i+1), 0);
        else
            q = reshape(q, r(i), n(i)*r(i+1));
            % Enrich by the extrema indices
            rr = zeros(n(i)*r(i+1), pos);
            if (pos>0)
                for j=1:pos
                    rr(tt_sub2ind([n(i) r(i+1)], ind_new(j,2:3)), j) = 1;
                end;
            end;
        end;
        % Enrich by random
        rr2 = randn(n(i)*r(i+1), kickrank*double(swp>0));
        [q,rv]=qr([q.',rr,rr2], 0);
        rv = rv(:,1:r(i)).';
        q = q.';
        rnew = size(q,1);
        % Maxvol
        ind = maxvol2(q.');
        qq = q(:, ind);
        q = qq\q;
        rv = rv*qq;
        % Indices
        ind_local{i} = ind; % for units
        ind_global{i} = indexmerge((1:n(i))', ind_global{i+1});
        ind_global{i} = ind_global{i}(ind,:);
        if (swp==0)
            tt{i} = reshape(q, rnew, n(i), r(i+1));
            % Cast rv to the next block
            q = reshape(tt{i-1}, r(i-1)*n(i-1), r(i));
            tt{i-1} = reshape(q*rv, r(i-1), n(i-1), rnew);
            r0(i) = rnew;
        end;
        r(i) = rnew;
        % Reduction
        XX0{i} = reshape(tt{i}, r0(i)*n(i), r0(i+1));
        XX0{i} = XX0{i}*XX0{i+1};
        XX0{i} = reshape(XX0{i}, r0(i), n(i)*r(i+1));
        XX0{i} = XX0{i}(:,ind);
        for j=1:numel(soughts)
            XU{i,j} = units{i,j}*XU{i+1,j};
            XU{i,j} = reshape(XU{i,j}, 1, n(i)*r(i+1));
            XU{i,j} = XU{i,j}(:, ind);
        end;
    end;

    i = i+dir;
    if ((dir>0)&&(i==d))||((dir<0)&&(i==1))
        dir = -dir;
        swp = swp+1;
    end;
end;

end

function [J]=indexmerge(varargin)
% Merges two or three indices in the little-endian manner
sz1 = max(size(varargin{1},1),1);
sz2 = max(size(varargin{2},1),1);
sz3 = 1;
if (nargin>2) % Currently allows only 3
    sz3 = max(size(varargin{3}, 1), 1);
end;
% J1 goes to the fastest index, just copy it
J1 = repmat(varargin{1}, sz2*sz3, 1);
% J2 goes to the middle
J2 = reshape(varargin{2}, 1, []);
J2 = repmat(J2, sz1, 1); % now sz1 ones will be the fastest
J2 = reshape(J2, sz1*sz2, []);
J2 = repmat(J2, sz3, 1);
J = [J1,J2];
if (nargin>2)
    % J3 goes to the slowest
    J3 = reshape(varargin{3}, 1, []);
    J3 = repmat(J3, sz1*sz2, 1); % now sz1 ones will be the fastest
    J3 = reshape(J3, sz1*sz2*sz3, []);
    J = [J,J3];
end;
end