function [distance,branch] = breadth(CIJ,source)
%BREADTH        Auxiliary function for breadthdist.m
%
%   [distance,branch] = breadth(CIJ,source);
%
%   Implementation of breadth-first search.
%
%   Input:      CIJ,        binary (directed/undirected) connection matrix
%               source,     source vertex
%
%   Outputs:    distance,   distance between 'source' and i'th vertex
%                           (0 for source vertex)
%               branch,     vertex that precedes i in the breadth-first search tree
%                           (-1 for source vertex)
%
%   Notes: Breadth-first search tree does not contain all paths (or all
%   shortest paths), but allows the determination of at least one path with
%   minimum distance. The entire graph is explored, starting from source
%   vertex 'source'.
%
%
%   Olaf Sporns, Indiana University, 2002/2007/2008

N = size(CIJ,1);

% colors: white, gray, black
white = 0;
gray = 1;
black = 2;

% initialize colors
color = zeros(1,N);
% initialize distances
distance = inf*ones(1,N);
% initialize branches
branch = zeros(1,N);

% start on vertex 'source'
color(source) = gray;
distance(source) = 0;
branch(source) = -1;
Q = source;

% keep going until the entire graph is explored
while ~isempty(Q)
   u = Q(1);
   ns = find(CIJ(u,:));
   for v=ns
% this allows the 'source' distance to itself to be recorded
      if (distance(v)==0)
         distance(v) = distance(u)+1;
      end;
      if (color(v)==white)
         color(v) = gray;
         distance(v) = distance(u)+1;
         branch(v) = u;
         Q = [Q v];                                             %#ok<AGROW>
      end;
   end;
   Q = Q(2:length(Q));
   color(u) = black;
end


function varargout = chebpolyval3(f, varargin)
%CHEBPOLYVAL3   Values of a CHEBFUN3 object F on a tensor product grid.
%   X = CHEBPOLYVAL3(F, M, N, P) returns an M x N x P tensor of values of a
%   CHEBFUN3 object F on a tensor product grid.
%
%   X = CHEBPOLYVAL3(F) does the same but sets M, N, and P equal to the
%   length of F.
%
%   [CORE, C, R, T] = CHEBPOLYVAL3(F) returns the low rank representation
%   of the values of F on a tensor product grid, i.e.,
%   X = CORE x_1 C x_2 R x_3 T.
%
%   Example: For an order-3 discrete tensor R, we should have
%                                       R \approx chebpolyval3(chebfun3(R)).

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

% Empty check.
if ( isempty(f) )
    varargout = { [] };
    return
end

if ( nargin == 1 )
    % Get degrees:
    [m, n, p] = length(f);
elseif ( nargin ~= 4 )
    error('CHEBFUN:CHEBFUN3:chebpolyval3:inputs', 'Dimension not specified.');
else
    m = varargin{1};
    n = varargin{2};
    p = varargin{3};
end

% Get low rank representation of f:
[core, cols, rows, tubes] = tucker(f);

tech = chebfunpref().tech();
colVals = tech.coeffs2vals(chebcoeffs(cols, m));
rowVals = tech.coeffs2vals(chebcoeffs(rows, n));
tubeVals = tech.coeffs2vals(chebcoeffs(tubes, p));

% Evaluate:
if ( nargout <= 1 )
    varargout = {chebfun3.txm(chebfun3.txm(chebfun3.txm(core, colVals, 1), ...
        rowVals, 2), tubeVals, 3)};
else
    varargout = {core, colVals, rowVals, tubeVals};
end

end


function sparse_unique_index = sgmga_unique_index ( dim_num, level_weight, ...
  level_max, rule, growth, np, p, tol, point_num, point_total_num )

%*****************************************************************************80
%
%% SGMGA_UNIQUE_INDEX maps nonunique to unique points of an SGMGA grid.
%
%  Discussion:
%
%    The sparse grid usually contains many points that occur in more
%    than one product grid.
%
%    When generating the point locations, it is easy to realize that a point
%    has already been generated.
%
%    But when it's time to compute the weights of the sparse grids, it is
%    necessary to handle situations in which weights corresponding to
%    the same point generated in multiple grids must be collected together.
%
%    This routine generates ALL the points, including their multiplicities,
%    and figures out a mapping from them to the collapsed set of unique points.
%
%    This mapping can then be used during the weight calculation so that
%    a contribution to the weight gets to the right place.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 April 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    An Anisotropic Sparse Grid Stochastic Collocation Method for Partial
%    Differential Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2411-2442.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real LEVEL_WEIGHT(DIM_NUM), the anisotropic weights.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Input, integer RULE(DIM_NUM), the rule in each dimension.
%     1, "CC",  Clenshaw Curtis, Closed Fully Nested.
%     2, "F2",  Fejer Type 2, Open Fully Nested.
%     3, "GP",  Gauss Patterson, Open Fully Nested.
%     4, "GL",  Gauss Legendre, Open Weakly Nested.
%     5, "GH",  Gauss Hermite, Open Weakly Nested.
%     6, "GGH", Generalized Gauss Hermite, Open Weakly Nested.
%     7, "LG",  Gauss Laguerre, Open Non Nested.
%     8, "GLG", Generalized Gauss Laguerre, Open Non Nested.
%     9, "GJ",  Gauss Jacobi, Open Non Nested.
%    10, "HGK", Hermite Genz-Keister, Open Fully Nested.
%    11, "UO",  User supplied Open, presumably Non Nested.
%    12, "UC",  User supplied Closed, presumably Non Nested.
%
%    Input, integer GROWTH(DIM_NUM), the growth in each dimension.
%    0, "DF", default growth associated with this quadrature rule;
%    1, "SL", slow linear, L+1;
%    2  "SO", slow linear odd, O=1+2((L+1)/2)
%    3, "ML", moderate linear, 2L+1;
%    4, "SE", slow exponential;
%    5, "ME", moderate exponential;
%    6, "FE", full exponential.
%
%    Input, integer NP(DIM_NUM), the number of parameters used by each rule.
%
%    Input, real P(*), the parameters needed by each rule.
%
%    Input, real TOL, the tolerance for point equality.
%
%    Input, integer POINT_NUM, the number of unique points in
%    the grid.
%
%    Input, integer POINT_TOTAL_NUM, the total number of points
%    in the grid.
%
%    Output, integer SPARSE_UNIQUE_INDEX(POINT_TOTAL_NUM), lists,
%    for each (nonunique) point, the corresponding index of the same point in
%    the unique listing.
%

%
%  Special cases.
%
  if ( level_max < 0 )
    sparse_unique_index = [];
    return
  end

  if ( level_max == 0 )
    sparse_unique_index(1) = 1;
    return
  end
%
%  Generate SPARSE_TOTAL_ORDER and SPARSE_TOTAL_INDEX arrays
%  for the TOTAL set of points.
%
  sparse_total_order = zeros(dim_num,point_total_num);
  sparse_total_index = zeros(dim_num,point_total_num);

  point_total_num2 = 0;
%
%  Initialization for SGMGA_VCN_ORDERED.
%
  level_weight_min_pos = r8vec_min_pos ( dim_num, level_weight );
  q_min = level_max * level_weight_min_pos - sum ( level_weight(1:dim_num) );
  q_max = level_max * level_weight_min_pos;
  level_1d_max = zeros(dim_num,1);
  for dim = 1 : dim_num
    if ( 0.0 < level_weight(dim) )
      level_1d_max(dim) = floor ( q_max / level_weight(dim) ) + 1;
      if ( q_max <= ( level_1d_max(dim) - 1 ) * level_weight(dim) )
        level_1d_max(dim) = level_1d_max(dim) - 1;
      end
    else
      level_1d_max(dim) = 0;
    end
  end
  more_grids = 0;
  level_1d = [];
%
%  Seek all vectors LEVEL_1D which satisfy the constraint:
%
%    LEVEL_MAX * LEVEL_WEIGHT_MIN_POS - sum ( LEVEL_WEIGHT )
%      < sum ( 1 <= I <= DIM_NUM ) LEVEL_WEIGHT(I) * LEVEL_1D(I)
%      <= LEVEL_MAX * LEVEL_WEIGHT_MIN_POS.
%
  while ( 1 )

    [ level_1d, more_grids ] = sgmga_vcn_ordered ( dim_num, level_weight, ...
      level_1d_max, level_1d, q_min, q_max, more_grids );

    if ( ~more_grids )
      break
    end
%
%  Compute the combinatorial coefficient.
%
    coef = sgmga_vcn_coef ( dim_num, level_weight, level_1d, q_max );

    if ( coef == 0.0 )
      continue
    end
%
%  Transform each 1D level to a corresponding 1D order.
%
    order_1d = level_growth_to_order ( dim_num, level_1d, rule, growth );
%
%  The inner loop generates a POINT of the GRID of the LEVEL.
%
    point_index = [];
    more_points = 0;

    while ( 1 )

      [ point_index, more_points ] = vec_colex_next3 ( dim_num, order_1d, ...
        point_index, more_points );

      if ( ~more_points )
        break;
      end

      point_total_num2 = point_total_num2 + 1;
      sparse_total_order(1:dim_num,point_total_num2) = order_1d(1:dim_num);
      sparse_total_index(1:dim_num,point_total_num2) = point_index(1:dim_num);

    end

  end
%
%  Now compute the coordinates of the TOTAL set of points.
%
  sparse_total_point = zeros(dim_num,point_total_num);
  sparse_total_point(1:dim_num,1:point_total_num) = r8_huge ( );

  level_weight_min_pos = r8vec_min_pos ( dim_num, level_weight );
  q_max = level_max * level_weight_min_pos;

  p_index = 1;

  for dim = 1 : dim_num

    if ( 0 < level_weight(dim) )
      level_1d_max(dim) = floor ( q_max / level_weight(dim) ) + 1;
      if ( q_max <= ( level_1d_max(dim) - 1 ) * level_weight(dim) )
        level_1d_max(dim) = level_1d_max(dim) - 1;
      end
    else
      level_1d_max(dim) = 0;
    end

    for level = 0 : level_1d_max(dim)

      order = level_growth_to_order  ( 1, level, rule(dim), growth(dim) );

      if ( rule(dim) == 1 )
        points = clenshaw_curtis_compute_points ( order );
      elseif ( rule(dim) == 2 )
        points = fejer2_compute_points ( order );
      elseif ( rule(dim) == 3 )
        points = patterson_lookup_points ( order );
      elseif ( rule(dim) == 4 )
        points = legendre_compute_points ( order );
      elseif ( rule(dim) == 5 )
        points = hermite_compute_points ( order );
      elseif ( rule(dim) == 6 )
        alpha = p(p_index);
        points = gen_hermite_compute_points ( order, alpha );
      elseif ( rule(dim) == 7 )
        points = laguerre_compute_points ( order );
      elseif ( rule(dim) == 8 )
        alpha = p(p_index);
        points = gen_laguerre_compute_points ( order, alpha );
      elseif ( rule(dim) == 9 )
        alpha = p(p_index);
        beta = p(p_index+1);
        points = jacobi_compute_points ( order, alpha, beta );
      elseif ( rule(dim) == 10 )
        points = hermite_genz_keister_lookup_points ( order );
      elseif ( rule(dim) == 11 )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'SGMGA_UNIQUE_INDEX - Fatal error!\n' );
        fprintf ( 1, '  Do not know how to assign points for rule 11.\n' );
        error ( 'SGMGA_UNIQUE_INDEX - Fatal error!' );
      elseif ( rule(dim) == 12 )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'SGMGA_UNIQUE_INDEX - Fatal error!\n' );
        fprintf ( 1, '  Do not know how to assign points for rule 12.\n' );
        error ( 'SGMGA_UNIQUE_INDEX - Fatal error!' );
      else
        fprintf ( 1, '\n' );
        fprintf ( 1, 'SGMGA_UNIQUE_INDEX - Fatal error!\n' );
        fprintf ( 1,'  Unexpected value of RULE = %d\n', rule(dim) );
        error ( 'SGMGA_UNIQUE_INDEX - Fatal error!' );
      end

      index = find ( sparse_total_order(dim,1:point_total_num) == order );

      sparse_total_point(dim,index) = points ( sparse_total_index(dim,index) );

    end

    p_index = p_index + np(dim);

  end
%
%  Merge points that are too close.
%
  seed = 123456789;

  [ point_num, undx, sparse_unique_index, seed ] = ...
    point_radial_tol_unique_index ( dim_num, point_total_num, ...
    sparse_total_point, tol, seed );

  for point = 1 : point_total_num
    rep = undx(sparse_unique_index(point));
    if ( point ~= rep )
      sparse_total_point(1:dim_num,point) = sparse_total_point(1:dim_num,rep);
    end
  end
%
%  Construct an index that indicates the "rank" of the unique points.
%
  [ undx, sparse_unique_index ] = point_unique_index ( dim_num, ...
    point_total_num, sparse_total_point, point_num );

  return
end


function anim = nrsfmXiaoKanade( W, nBasis )
% Compute orthographic non-rigid structure from motion using Xiao-Kanade
%
% Just check the Xiao's CVPR04 paper
%
% USAGE
%  anim = nrsfmXiaoKanade( W, nBasis )
%
% INPUTS
%  W             - [ 2 x nPoint x nFrame ] set of 2D points
%  nBasis        - {Computed } number of bases to use
%
% OUTPUTS
%  anim          - Animation object (help Animation for details)
%
% EXAMPLE
%
% See also COMPUTESMFROMW
%
% Vincent's Structure From Motion Toolbox      Version 3.0
% Copyright (C) 2008-2010 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]

nFrame = size( W, 3 ); nPoint = size( W, 2 );
animBest=Animation; animBest.W = W;

W = reshape( permute( W, [ 1 3 2 ] ), [], nPoint );
animBest.t = reshape( mean(W,2), 2, nFrame ); animBest.t(3,:) = 0;
W = bsxfun(@minus,W,mean(W,2));

if nargin<2 || isempty(nBasis)
  [ U S V ] = svd(W,'econ'); S = diag(S); sumVal = S(1); Kd = 1;
  while ( Kd<=length(S) ) && ( sumVal < 0.995*sum(abs(S)) )
    if S(Kd)<0; break; end
    Kd = Kd + 1; sumVal = sumVal + S(Kd);
  end
  W = U(:,1:Kd)*diag(S(1:Kd))*V(:,1:Kd)';
else
  Kd = 3*nBasis;
  % Check if Kd is not smaller (degenerate deformations)
  [ U S V ] = svd(W,'econ');
  S=cumsum(diag(S));
  ind = find(S>=0.995*S(end));
  Kd=min(Kd,ind(1));
end

%%%%%%%%%%%%%%%%%%%%%% Determine K3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K3 = floor(Kd/3);
% Basis constraints
WTildeOri = [ W(1:2:end,:) W(2:2:end,:) ];

while 1
  % Perform random search for 5 seconds
  t0 = clock; mini = Inf;
  while etime( clock, t0 )<5
    indWTmp = randSample(nFrame,K3);
    tmp = cond( WTildeOri( indWTmp, : ) );
    if tmp < mini; mini = tmp; indW = indWTmp; end
  end

  if mini>1e2 && K3>0; K3=K3-1; else break; end
end

%%%%%%%%%%%%%%%%%%%%%% Deal with the K3 basis %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute initial variables
[ MTilde S disc ] = svd( W );
MTilde = MTilde( :, 1:Kd )*S(1:Kd,1:Kd); gj3=zeros(Kd,3,K3);

% Rotation constraints
Qi = sdpvar( Kd, Kd );
F = set( Qi >= 0 );
% vect(AXB)=kron(B',A)*vect(X)
kron1=zeros(nFrame,Kd^2); kron2=zeros(nFrame,Kd^2);
for i=1:nFrame
  kron1(i,:)=kron(MTilde(2*i-1,:),MTilde(2*i-1,:)) - ...
    kron(MTilde(2*i,:),MTilde(2*i,:));
  kron2(i,:)=kron(MTilde(2*i,:),MTilde(2*i-1,:));
end
objRotation=abs(kron1*Qi(:))+abs(kron2*Qi(:));

% Solve for Qk using all the constraints
kk = 1;
for i = indW
  nc = 2*nFrame + 1;
  objBasis = 0;
  disp('Computing the basis constraints');
  for m = indW
    if m==i; interN = m; else interN = 1 : nFrame; end
    for n = interN % could be sampled
      if m==n && m==i
        objBasis = objBasis +abs(MTilde(2*m-1,:)*Qi*MTilde(2*n-1,:)'-1) ...
          + abs( MTilde(2*m,:)*Qi*MTilde(2*n,:)' - 1);
      else
        objBasis = objBasis +abs(MTilde(2*m-1,:)*Qi*MTilde(2*n-1,:)') ...
          + abs( MTilde(2*m,:)*Qi*MTilde(2*n,:)' );
      end

      objBasis = objBasis + abs( MTilde(2*m-1,:)*Qi*MTilde(2*n,:)' ) + ...
        abs( MTilde(2*m,:)*Qi*MTilde(2*n-1,:)' );
    end
  end

  % Sample the constraints
  disp('Solving the SDP');
  diagno = solvesdp( F,objRotation+objBasis,sdpsettings('solver',...
    'sdpa,csdp,sedumi,*','dualize',1,'verbose',0));

  QiTmp = double(Qi);

  [ U S disc ] = svd( QiTmp ); gj3(:,:,kk) = U(:,1:3)*sqrt(S(1:3,1:3));
  kk = kk + 1;
end

%  [U S V]=svd(constr,'econ');
%  rank(constr)
%  size(null(constr(1:2*nFrame,:)))
%  size(constr)
%  tmp=diag(S)';
%  tmp(end-3:end)
%  size(null(constr),2)
%  return
%%%%%%%%%%%%%%%%%%%%%% Get K2 and K1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% The explanations given by Xiao04 seems wrong to determine K1 and K2
%% So, let's do it the lazy way: let's try all possible combinations of K1
%% and K2
errBest = Inf;
fprintf('\nSo far, Kd=%d, K3=%d\n', Kd, K3 );
for K2 = floor((Kd-3*K3)/2) : -1 : 0
  K1 = Kd - 3*K3 - 2*K2;
  K = K3+K2+K1;

  fprintf('\nTrying, Kd=%d, K1=%d, K2=%d, K3=%d\n', Kd, K1, K2, K3 );

  %%%%%%%%%%%%%%%%%%%%%% Get the sets of rotations %%%%%%%%%%%%%%%%%%%%%%%%
  G = reshape( gj3, Kd, [] );
  M = MTilde*G; M = M/max(abs(M(:)))*K3; % Just for numerical stability
  R = zeros( 2*nFrame, 3*K3 ); l = zeros( K, nFrame ); thres = 0.001;
  rotIsBad = zeros( K3, nFrame );
  for i = 1 : nFrame
    for k = 1 : K3
      tmp = M(2*i-1:2*i,3*k-2:3*k);
      l(k,i) = norm(tmp);

      if abs( l(k,i) )>thres; R(2*i-1:2*i,3*k-2:3*k) = tmp/l(k,i);
      else rotIsBad(k,i)=1;
      end
    end
  end

  % Rectify R (not really mentioned by Xiao)
  for k=2:K3
    for i = 2:nFrame
      goodFrame = find( sum( rotIsBad([1 k], 1:i), 1 )==0);
      goodFrame = sort( [ 2*goodFrame 2*goodFrame-1 ] );
      if length(goodFrame)<4; continue; end
      R1 = R( goodFrame, 1:3 );
      R2 = R( goodFrame, 3*k-2:3*k );
      R3 = R2; R3(end-1:end,:) = -R3(end-1:end,:);

      if norm( R2*(R2\R1)-R1, 'fro' )>norm( R3*(R3\R1)-R1, 'fro' )
        R( goodFrame(end-1:end), 3*k-2:3*k ) = ...
          -R( goodFrame(end-1:end), 3*k-2:3*k );
      end
    end

    goodFrame = find( sum( rotIsBad([1 k], 1:nFrame), 1 )==0);
    goodFrame = sort( [ 2*goodFrame 2*goodFrame-1 ] );
    R1 = R( goodFrame, 1:3 );
    R2 = R( goodFrame, 3*k-2:3*k );
    tmp=R2\R1;

    R( :, 3*k-2:3*k ) = R( :, 3*k-2:3*k )*tmp;
    M( :, 3*k-2:3*k ) = M( :, 3*k-2:3*k )*tmp;
  end

  % Get all the rotations and coefficients in K3 (if non-degenerate)
  RTot = zeros(3,3,nFrame);
  for i = 1 : nFrame
    % Get the average rotation matrix
    tmp=find(~rotIsBad(:,i)');
    if isempty(tmp); continue; end
    RTmp=zeros(3,3,length(tmp));

    qTot=cell(1,2);
    for k=1:2
      kk=1;
      for j=tmp
        RTmp(:,:,kk)=rotationMatrix( R(2*i-1:2*i,3*j-2:3*j)*(2*k-3) );
        kk=kk+1;
      end

      q=quaternion(RTmp);
      for j=2:size(q,2)
        if norm(-q(:,j)-q(:,1))<norm(q(:,j)-q(:,1)); q(:,j)=-q(:,j); end
      end
      qTot{k}=mean(q,2);
    end

    q=qTot{1};
    if i>=2
      q0 = quaternion( RTot(:,:,i-1) );
      if min( norm(qTot{2}-q0), norm(-qTot{2}-q0) )<...
          min( norm(qTot{1}-q0), norm(-qTot{1}-q0) )
        q=qTot{2};
      end
    end

    RTot(:,:,i) = quaternion(q);

    % Recover the coefficients
    l(1:K3,i)=(reshape(RTot(1:2,:,i),[],1)\reshape(M(2*i-1:2*i,1:3*K3),6,K3))';

    % Compute MTilde
    MTilde(2*i-1:2*i,1:3*K3)=kron(l(1:K3,i)',RTot(1:2,:,i));
  end
  for k = 1 : K3
    gj3(:,:,k) = 0; gj3(3*k-2:3*k,:,k) = eye(3);
  end
  R = RTot;
  rotIsUnknown = sum(abs(l(1:K3,:))<thres,1)==K3;
  if sum(~rotIsUnknown)==0
    [disc indMax]=max(sum(l(1:K3,:).^2,1)); rotIsUnknown(indMax)=1;
  end
  goodFrame = find( ~rotIsUnknown );
  tmp=sort([2*goodFrame,2*goodFrame-1]);

  % Get the MTilde where l(1:K3,i) is close to 0
  M=MTilde(tmp,:); B=M\W(tmp,:); M2=W/B;
  for i=find(rotIsUnknown)
    MTilde(2*i-1:2*i,:)=M2(2*i-1:2*i,:);
  end

  %   find(rotIsUnknown)
  %   plot(l')
  %     B=MTilde\W;
  %   norm(MTilde*B-W,'fro')
  %   pause

  % The first 3*K3 columns of MTilde and MHat are now identical
  % We now need to deal with the last columns

  %%%%%%%%%%%%%%%%%%%%%% Find gj and rj %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % We here perform a full optimization and not the hacky alternate
  % optimization from the paper
  MNull = zeros( 2*K3, Kd );
  for i = 1 : K3
    MNull(2*i-1:2*i,:) = MTilde( 2*indW(i)-1:2*indW(i), : );
  end
  [ U S V ] = svd( MNull, 'econ' );
  [ V disc ] =rq(V);
  gj2 = reshape( V(:,end-(2*K2+K1)+1:end-K1), [], 2, K2);rj2=zeros(3,2,K2);
  gj1 = V(:,end-K1+1:end); rj1=zeros(3,K1);

  % Perform iterations for K2 bases
  opt = optimset( 'Display', 'on', 'GradObj', 'off', 'Hessian', 'off',...
    'LargeScale', 'off' );
  for j=1:K2
    % Get the best rj2
    ATmp=zeros(4*length(goodFrame),3);
    for kk=1:2
      n = 1;
      for m=goodFrame
        for k=1:2
          ATmp(n,:)=gj2(:,k,j)'*MTilde(2*m-1,:)'*R(2,:,m) - ...
            gj2(:,k,j)'*MTilde(2*m,:)'*R(1,:,m);
          n = n + 1;
        end
      end

      % Force the rj2 to be independent
      zeroStart=2+kk;
      [ U S V ] = svd( ATmp(:,1:zeroStart-1), 'econ' );
      rj2(1:zeroStart-1,kk,j) = V(:,end)/V(end,end);
      rj2(zeroStart:end,kk,j) = 0;
    end

    % Perform the full optimization
    onePos=3*K3+1+2*(j-1);
    x=[gj2(1:onePos-1,1,j); gj2(1:onePos,2,j); rj2(1:2,1,j); rj2(1:2,2,j)];
    MTildeTmp= MTilde(:,1:onePos+1); MNullTmp=MNull(:,1:onePos+1);
    x = fminunc( @(X)( optimizeGj2( X,goodFrame, MTildeTmp, ...
      R, MNullTmp ) ), x,opt);

    % Save the best results
    rj2(:,:,j)=[x(end-3:end-2) x(end-1:end); 0 1 ];
    gj2(:,:,j)=0;
    gj2(1:onePos,1,j) = [ x(1:onePos-1); 1 ];
    gj2(1:onePos+1,2,j) = [ x(1:onePos); 1 ];
  end

  % Perform iterations for K1 bases
  for j=1:K1
    % Get the best rj1
    ATmp=zeros(length(goodFrame),3);
    for m=goodFrame
      ATmp(m,:) = gj1(:,j)'*( MTilde( 2*m-1 , : )'*R(2,:,m) - ...
        MTilde( 2*m , : )'*R(1,:,m) );
    end

    [ U S V ] = svd( ATmp, 'econ' );
    rj1(:,j) = V(:,end);

    % Perform the full optimization
    onePos=3*K3+2*K2+j;
    x=[gj1(1:onePos-1,j); rj1(:,j) ];
    MTildeTmp= MTilde(:,1:onePos); MNullTmp=MNull(:,1:onePos);
    x = fminunc( @(X)( optimizeGj1( X,goodFrame, MTildeTmp, ...
      R, MNullTmp ) ), x,opt);

    % Save the best results
    rj1(:,j)=x(end-2:end);
    gj1(:,j)=0; gj1(1:onePos,j) = [ x(1:onePos-1); 1 ];
  end

  % % quality
  % err=zeros(1,length(goodFrame));
  % for j = 1:K2
  %   for m=goodFrame
  %     err(m)=err(m)+abs(gj2(:,1,j)'*( MTilde( 2*m-1 , : )'*R(2,:,m) - ...
  %       MTilde( 2*m , : )'*R(1,:,m) )*rj2(:,1,j)) ...
  %        + abs(gj2(:,2,j)'*( MTilde( 2*m-1 , : )'*R(2,:,m) - ...
  %       MTilde( 2*m , : )'*R(1,:,m) )*rj2(:,2,j));
  %   end
  % end
  %       norm(err)
  % %       norm(MNull*gj1(:,j))
  %       plot(err)
  %
  % pause

  %%%%%%%%%%%%%%%%%%%%%% Recover the coefficients for K2 K1 %%%%%%%%%%%%%%%
  % Rectify the right columns of MTilde
  for m=1:nFrame
    for i = 1 : K2
      MTilde(2*m-1:2*m,3*K3+2*i-1:3*K3+2*i)=MTilde(2*m-1:2*m,:)*gj2(:,:,i);
    end
    for i = 1 : K1
      MTilde(2*m-1:2*m,3*K3+2*K2+i)=MTilde(2*m-1:2*m,:)*gj1(:,i);
    end
  end
  %   l(K3+i,m)*R(1:2,:,m)*rj2(:,:,i)=
  %                   MTilde(2*m-1:2*m,3*K3+2*i-1:3*K3+2*i)*RAmbiguityi
  %   l(K3+i,m)*R(1:2,:,m)*rj1(:,i)=
  %                   MTilde(2*m-1:2*m,3*K3+2*K2+i)
  for m=goodFrame
    for i = 1 : K2
      tmp1=R(1:2,:,m)*rj2(:,:,i); tmp1=tmp1*tmp1';
      tmp2=MTilde(2*m-1:2*m,3*K3+2*i-1:3*K3+2*i); tmp2=tmp2*tmp2';
      l(K3+i,m) = sqrt( tmp1(:)\tmp2(:) );
    end
    for i = 1 : K1
      tmp1=R(1:2,:,m)*rj1(:,i);
      tmp2=MTilde(2*m-1:2*m,3*K3+2*K2+i);
      l(K3+K2+i,m) = tmp1\tmp2;
    end
  end

  %   B=MTilde\W;
  %   norm(MTilde*B-W,'fro')
  %   plot(l')
  %   pause

  % Change the signs of the coefficients
  for i = 1 : K2
    A=zeros(2*nFrame,2);
    A(1:2,:)=l(K3+i,goodFrame(1))*R(1:2,:,goodFrame(1))*rj2(:,:,i);

    for j=2:length(goodFrame)
      m=goodFrame(j); A(2*j-1:2*j,:)=l(K3+i,m)*R(1:2,:,m)*rj2(:,:,i);
      tmp=goodFrame(1:j); tmp=sort( [ 2*tmp-1, 2*tmp ] );
      MTildeLoc=MTilde(tmp,3*K3+2*i-1:3*K3+2*i);

      RAmb1=MTildeLoc\A(1:2*j,:);
      err1=norm(MTildeLoc*RAmb1-A(1:2*j,:),'fro');

      A(2*j-1:2*j,:)=-A(2*j-1:2*j,:);
      RAmb2=MTildeLoc\A(1:2*j,:);
      err2=norm(MTildeLoc*RAmb2-A(1:2*j,:),'fro');

      if err2<err1; l(K3+i,m)=-l(K3+i,m);
      else A(2*j-1:2*j,:)=-A(2*j-1:2*j,:);
      end
    end
  end

  B=MTilde\W;
  %   norm(MTilde*B-W,'fro')
  %   plot(l')
  %   pause

  %%%%%%%%%%%%%%%%%%%%%% Recover the shape basis %%%%%%%%%%%%%%%%%%%%%%%%%%
  M=zeros(2*length(goodFrame),3*K);
  for i=1:length(goodFrame)
    M(2*i-1:2*i,:)=kron(l(:,goodFrame(i))',R(1:2,:,goodFrame(i)));
  end

  tmp=sort([2*goodFrame,2*goodFrame-1]);
  B=M\W(tmp,:);
  %   norm(M*B-W(tmp,:),'fro')
  %   tmp=sum((M*B-W(tmp,:)).^2,2);
  %   plot( tmp(1:2:end)+tmp(2:2:end) )
  %   'coiin'
  %   pause

  % Recover the full basis
  SBasis = zeros( 3, nPoint, K );
  for i = 1 : K; SBasis(:,:,i) = B(3*i-2:3*i,:); end

  %%%%%%%%%%%%%%%%%%%%%% Recover the missing coefficients %%%%%%%%%%%%%%%%%
  opt = optimset( 'Display', 'off', 'GradObj', 'off', 'Hessian', 'off',...
    'LargeScale', 'off' );

  while 1
    for m=1:nFrame
      if rotIsUnknown(m)
        if m==1 || rotIsUnknown(m-1)
          if m==nFrame; continue
          else if ~rotIsUnknown(m+1); iIni=m+1; else continue; end
          end
        else iIni=m-1;
        end

        % Optimize over the shape coefficients/rotation
        coeff=[ l(:,iIni); quaternion( R(:,:,iIni) ) ];
        Wm=W(2*m-1:2*m,:);

        [ c res ] = fminunc( @(X)( optimRl( X, K,B,Wm ) ), coeff, opt );

        l(:,m)=c(1:K); R(:,:,m)=quaternion(c(K+1:end));
        rotIsUnknown(m)=0;
      else
        if m>=2
          %           plot(quaternion(R)')
          %           pause
          q0 = quaternion( R(:,:,m-1) );
          q1 = quaternion( R(:,:,m) );
          q2 = quaternion( rotationMatrix( -R(1:2,:,m) ) );
          if min( norm(q2-q0), norm(-q2-q0) )<min(norm(q1-q0),norm(-q1-q0))
            l(:,m)=-l(:,m); R(:,:,m)=rotationMatrix(-R(1:2,:,m));
          end
        end
      end
    end
    if ~rotIsUnknown(1); break; end
  end

  %%%%%%%%%%%%%%%%%%%%%% Finalize the anim object %%%%%%%%%%%%%%%%%%%%%%%%%
  anim=Animation('SBasis', SBasis, 'l', l, 'R', R, 'isProj', false, ...
    't', animBest.t, 'W', animBest.W );
  err = anim.computeError(); err=err(1);
  fprintf( 'Reprojection error: %f \n', err );

  if err<errBest; errBest=err; animBest=anim; end
end
anim=animBest;

fprintf( 'Best reprojection error: %f \n', errBest );
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function res = optimRl(X,K,B,Wm)
lm=X(1:K);
Rm=quaternion(X(K+1:end)/norm(X(K+1:end)));
res = norm(kron(lm',Rm(1:2,:))*B-Wm,'fro')^2;
end

function val = optimizeGj2( x, goodFrame, MTilde, R, MNull )
% gjk' MTilde_{2m-1}' R_m2 rjkk - gjk' MTilde_2m' R_m1 rjkk = 0,
% k and kk in {1 2}
% rj can be multiplied by any 2x2
rj2=[x(end-3:end-2) x(end-1:end); 0 1 ]; rj2(:,1)=rj2(:,1)/norm(rj2(:,1));
n=(length(x)-4-1)/2;
gj2=[ [ x(1:n); 1; 0 ] [ x(n+1:2*n+1); 1 ] ];
n=n+2;

n=1;
val=zeros(1,length(goodFrame));
A=gj2'*MTilde';
for m=goodFrame
  val(n)=norm( ( A(:,2*m-1,:)*R(2,:,m) - A(:,2*m)*R(1,:,m) )*rj2, 'fro' );
  n=n+1;
end
val=norm([ val reshape(MNull*gj2,[],1)' ]);
end

function val = optimizeGj1( x, goodFrame, MTilde, R, MNull )
% gj' ( MTilde_{2m-1} R_m2 - MTilde_2m R_m1 ) rj = 0
gj1=[ x(1:end-3); 1 ];
rj1=x(end-2:end); rj1=rj1/norm(rj1);

n=1;
A=gj1'*MTilde';
B=zeros(length(goodFrame),3);
for m=goodFrame
  B(n,:)=A(:,2*m-1)*R(2,:,m) - A(:,2*m)*R(1,:,m);
  n=n+1;
end
val=norm([ (B*rj1); reshape(MNull*gj1,[],1) ]);

end


function W=RegCsp(EEGdata,gnd,genSs,genMs,beta,gamma)
% RCSP: Regularized Common Spatial Pattern
%
% %[Prototype]%
% function W=RegCsp(EEGdata,gnd,genSs,genMs,beta,gamma)
%
% %[Author Notes]%
% Author: Haiping LU
% Email : hplu@ieee.org   or   eehplu@gmail.com
% Release date: March 20, 2012 (Version 1.0)
% Please email me if you have any problem, question or suggestion
%
% %[Algorithm]%:
% This function implements the Regularized Common Spatial Pattern
% (R-CSP) algorithm presented in the follwing paper:
%    Haiping Lu, How-Lung Eng, Cuntai Guan, K.N. Plataniotis, and A.N. Venetsanopoulos,
%    "Regularized Common Spatial Pattern With Aggregation for EEG Classification in Small-Sample Setting",
%    IEEE Trans. on Biomedical Engineering, Vol. 57, No. 12, Pages
%    2936-2946, Dec. 2010.
% Please reference this paper when reporting work done using this code.
%
% The following is an earlier conference version
%    Haiping Lu, K.N. Plataniotis, and A.N. Venetsanopoulos,
%    "Regularized Common Spatial Patterns with Generic Learning for EEG Signal Classification",
%    in Proceedings of the 31st Annual International Conference of the
%    IEEE Engineering in Medicine and Biology Society (EMBC), Sep., 2009.
%
% %[Syntax]%: W=RegCsp(EEGdata,gnd,genSs,genMs,beta,gamma)
%
% %[Inputs]%:
%    EEGdata: the input EEG data for training, a 3D array with size [numT,numCh,nTrl]
%       numT is the number of samples in each channle (T in the paper)
%       numCh is the number of channels (N in the paper)
%       nTrl is the number of trails for training (2*M in the paper)
%
%    gnd: the ground truth class labels for the nTrl trials, size nTrl x 1
%
%    genSs: size numCh x numCh x 2, the sum of generic covariance matrices
%       from generic training trials for two classes, equation (6) in the paper
%
%    genMs: size 2 x 1, the number of generic training trials for two classes
%
%    beta,gamma: the reguarlizatio parameters in the paper, setting both to
%       zero gives the conventional CSP
%
% %[Outputs]%:
%    W: \hat{W} in the paper, the projection matrix, we often use only the
%         first 6 columns from W
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %[Notes]%:
% A. Developed using Matlab R2006a
% B. Revision history:
%       Version 1.0 released on March 20, 2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%
[numT,numCh,nTrl] = size(EEGdata);%Input data, see documentation
Ns=zeros(2,1);
Sigma2=cell(2,1);%Two average spatial covariance matrices, one for each class
%%%%%%%%%%%%%%%%%%%
for i=1:2%for each class
    Idxs=find(gnd==i);
    EEG=EEGdata(:,:,Idxs);
    Ns(i)=length(Idxs);
    C=zeros(numCh,numCh,Ns(i));%Sample covariance matrix, equation (1) in the paper
    for trial=1:Ns(i)
        E=EEG(:,:,trial)';
        tmpC = (E*E');
        C(:,:,trial) = tmpC./trace(tmpC);%normalization
    end
    Csum=sum(C,3);
    %Reguarlization, see equations (3) and (4) in the paper
    Sigma1=((1-beta)*Csum+beta*genSs(:,:,i))/((1-beta)*Ns(i)+beta*genMs(i));
    Sigma2{i}=(1-gamma)*Sigma1+gamma*trace(Sigma1)*eye(numCh)/numCh;
end

%Equation (7) in the paper
SigmaComps=Sigma2{1}+Sigma2{2};
[Ucomps,lmds] = eig(SigmaComps);
[lmds,Idxs] = sort(diag(lmds),'descend');
Ucomps = Ucomps(:,Idxs);

%Note equations (8) to (12) in the conference version is now condensed in
%one equation (8) in the journal version
%Equation (8) in the CONFERENCE paper
P=sqrt(inv(diag(lmds)))*Ucomps';

%Equation (9) in CONFERENCE the paper
Sgm1=P*Sigma2{1}*P';

%Equation (11) in the CONFERENCE paper
[B,D] = eig(Sgm1);
[D,Idxs] = sort(diag(D),'descend');
B = B(:,Idxs);

%Equation (12) in the CONFERENCE paper
%Equation (8) in the JOURNAL paper
W=(B'*P);
%Normalize the projrection matrix
for i=1:length(Idxs), W(i,:)=W(i,:)./norm(W(i,:)); end

%Sort columns, take first and last columns first, etc
W0=W;
W=zeros(size(W));
i=0;
for d=1:numCh
    if (mod(d,2)==0)
        W(d,:)=W0(numCh-i,:);
        i=i+1;
    else
        W(d,:)=W0(1+i,:);
    end
end


function [A,B,C,D] = vibsHmxBlockOperator(omega,U,sigma,u,cL,cT,rhoS,c0,rho0,f,tol)
%+========================================================================+
%|                                                                        |
%|                 OPENVIBS - LIBRARY FOR VIBRO-ACOUSTIC                  |
%|           openVibs is part of the GYPSILAB toolbox for Matlab          |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal, Marc Bakry (c) 2017-2019.                 |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             marc.bakry@polytechnique.edu                               |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : vibsHmxBlockOperator.m                        |
%|    #    |   VERSION    : 0.55                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & Marc Bakry                  |
%|  ( # )  |   CREATION   : 14.03.2019                                    |
%|  / 0 \  |   LAST MODIF :                                               |
%| ( === ) |   SYNOPSIS   :                                               |
%|  `---'  |                                                              |
%+========================================================================+

% Constants
mu     = rhoS*cT^2;
lambda = rhoS*(cL^2 - 2*cT^2);
w      = 2*pi*f;
k      = w/c0;

% Dimension
n = size(omega.msh.elt,2)-1;

% Green kernel function
if (n == 2)
    Gxy         = @(X,Y) femGreenKernel(X,Y,'[H0(kr)]',k);
    gradyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]1',k);
    gradyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]2',k);
    gradyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]3',k);
    G0          = '[log(r)]';
    gradyG0     = 'grady[log(r)]';
    cteGxy      = 1i/4;
    cteG0       = -1/(2*pi);

elseif (n == 3)
    Gxy         = @(X,Y) femGreenKernel(X,Y,'[exp(ikr)/r]',k);
    gradyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]1',k);
    gradyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]2',k);
    gradyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]3',k);
    G0          = '[1/r]';
    gradyG0     = 'grady[1/r]';
    cteGxy      =  1/(4*pi);
    cteG0       =  1/(4*pi);

else
    error('vibsNeumannBW.m : unavailable case.')
end

% Coupling coeff for Brackage-Werner simulation
beta = 1i*k;

%%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ELASTO (A11) %%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialization
A = cell(n,n);

% Static part
GG = integral(omega,grad(U),grad(U));
for i = 1:n
    for j = 1:n
        % Operator div(U):div(U)
        DD = integral(omega,grad(U,i),grad(U,j));

        % Operator e(U):e(U)
        EE = integral(omega,grad(U,j),grad(U,i));
        if (i==j)
            EE = EE + GG;
        end

        % Summation
        A{i,j} = lambda.*DD + mu.*EE;
    end
end

% Dynamic part
if (f ~= 0)
    Id = integral(omega,U,U);
    for i = 1:n
       A{i,i} = A{i,i} - (rhoS*w^2) .* Id;
    end
end

% Final form (sparse)
A = cell2mat(A);

%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ACOUSTIC (A12) %%%%%%%%%%%%%%%%%%%%%%%%%%

% Gaussian quadrature
[Xqud,Wx] = sigma.qud;
Wx        = spdiags(Wx,0,length(Wx),length(Wx));

% Collocation mass operator
Id = u.uqm(sigma);

% Collocation boundary operator
S = cteGxy .* integral(Xqud,sigma,Gxy,u,tol) + ...
    cteG0  .* regularize(Xqud,sigma,G0,u);

% Collocation boundary operator
D = cteGxy .* integral(Xqud,sigma,gradyGxy,ntimes(u),tol) + ...
    cteG0  .* regularize(Xqud,sigma,gradyG0,ntimes(u));

% Final operator Brackage-Werner : [1i*k*beta*S - (Id/2 + D)]
B.Mr = beta.*S - (0.5*Id + D);

% Normal trace of the volumn element matrix
nU   = ntimes(U);
nPHI = nU.uqm(sigma);

% Coupling to FEM
B.Ml = cell(n,1);
for i = 1:n
    B.Ml{i} = (nPHI{i}' * Wx);
end
B.Ml = cell2mat(B.Ml);

%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ACOUSTIC (A21) %%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialization
C = cell(1,n);

% Coupling FEM
for i = 1:n
    C{i} = (rho0*w^2) .* integral(sigma,u,ntimes(U,i));
end

% Final format (sparse)
C = cell2mat(C);

%%%%%%%%%%%%%%%%%%%%%%%%%% ACOUSTIC-ACOUSTIC (A22) %%%%%%%%%%%%%%%%%%%%%%%%

% Finite element mass matrix
Id = integral(sigma,u,u);

% Finite element boundary operator
H  = cteGxy .* (k^2 * integral(sigma,sigma,ntimes(u),Gxy,ntimes(u),tol) ...
    - integral(sigma,sigma,nxgrad(u),Gxy,nxgrad(u),tol));
Hr = cteG0  .* (k^2 * regularize(sigma,sigma,ntimes(u),G0,ntimes(u)) ...
    - regularize(sigma,sigma,nxgrad(u),G0,nxgrad(u)));

% Finite element boundary operator
D  = cteGxy .* integral(sigma,sigma,u,gradyGxy,ntimes(u),tol);
Dr = cteG0  .* regularize(sigma,sigma,u,gradyG0,ntimes(u));

% Final operator Brackage-Werner : - [1i*k*beta*(-Id/2 + Dt) - H]
D = - (beta.*(-0.5*Id + (D+Dr).') - (H+Hr));
end


function [tq,q,tu,u,t,p,Q,code]=hydrograph(uhname,rdname,method,...
                                           K,D,R,Iar,A,CN,Tc,units)
% SCS Unit Hydrograph Convolution (R11)
%
% [tq,q,tu,u,t,p,Q,code]=hydrograph(uhname,rdname,method,...
%                                   K,D,R,Iar,A,CN,Tc,units)
%
% Input
%   uhname  dimensionless unit hydrograph filename
%   rdname  dimensionless rainfall distribution filename
%   method  interpolation method
%   K       peak factor
%   D       storm duration (hr)
%   R       rainfall depth (mm | in)
%   Iar     initial abstraction ratio
%   A       basin area (km^2 | ac)
%   CN      curve number
%   Tc      time of concentration (min)
%   units   units code
%             0 = imperial
%             1 = metric
%
% Output
%   tq      runoff hydrograph time (hr)
%   q       runoff hydrograph flow rate (cms | cfs)
%   tu      unit hydrograph time (hr)
%   u       unit hydrograph flow rate (cms | cfs)
%   t       rainfall-runoff time (hr)
%   p       cumulative rainfall (mm | in)
%   Q       cumulative runoff (mm | in)
%   code    return code
%             0 = fail
%             1 = pass
%
% Example:
%
%  uhname='library\gamma.duh'; rdname='library\scsii-024.drd'; K=484;
%  method='linear'; D=24; R=200; Iar=0.2; A=3; CN=75; Tc=90; units=1;
%  [tq,q,tu,u,t,p,Q,code]=hydrograph(uhname,rdname,method,...
%                                    K,D,R,Iar,A,CN,Tc,units)
%
% See also help\hydographui.html.

% Version 2.03 Copyright(c)2008
% Tom Davis (tdavis@metzgerwillard.com)
%
% Last revision: 03/16/2008

if units
  R=R/25.4;                             % rainfall depth (in)
  cm2cf=35.3146667214886;               % cubic meters to cubic feet
  %    =1e6/(12^3*2.54^3);
  sk2ac=247.105381467165;               % square kilometers to acres
  %    =1e10/(43560*12^2*2.54^2);
  A=A*sk2ac;                            % basin area (ac)
end
A =A/640;                               % basin area (mi^2)
Tc=Tc/60;                               % time of concentration (hr)
d =Tc/7.5;                              % rain burst duration (hr)
Tp=5*d;                                 % time to peak (hr)
method1=lower(method);
method2=method1;
uhname=lower(uhname);
code=1;

if ~isempty(findstr('triangle',uhname))
  uhname='triangle';
elseif ~isempty(findstr('gamma',uhname))
  uhname='gamma';
end

switch uhname                           % dimensionless unit hydrograph
  case 'triangle'
    Tb=3872/(3*K);                      % time base
    % =2(5280^2/60^2/12)/K
    uh=[0,0;1,1;Tb,0];                  % triangular distribution
    method1='linear';
  case 'gamma'
    f =K*3/1936;
    % =K/(5280^2/60^2/12)
    a0=0.045+0.5*f+5.6*f^2+0.3*f^3;     % cubic estimate
    options=optimset('display','off','tolx',eps);
    g =inline('gamma(a)*exp(a)/a^a-1/f','a','f');
    a =fzero(g,a0,options,f);
    tu=(0:0.2:round(2500/K))';
    u =(tu.*exp(1-tu)).^a;              % gamma distribution
    u(end)=0;                           % force zero ordinate
  otherwise
    uh=load(uhname);                    % tabular distribution
    [m,n]=size(uh);
    [umax,ndx]=max(uh(:,2));            %#ok umax not used
    umin=min(min(uh));
    if m<3 | n~=2 | umin<0 | uh(1,:)~=[0 0] | uh(ndx,:)~=[1 1] %#ok (R11)
      msgbox('Unit Hydrograph is improperly formed.',...
        'File Error','error','modal');
      tq=[];q=[];tu=[];u=[];t=[];p=[];Q=[];code=0;
      return
    end
end

if ~strcmp(uhname,'gamma')
  uh=[uh;uh(end,1)+0.2,0];              % force zero ordinate
  Tu=uh(:,1);
  U =uh(:,2);
  % resample unit hydrograph
  tu=(0:0.2:Tu(end))';
  u =interp1(Tu,U,tu,method1);
end

tu=Tp*tu;                               % unit hydrograph time (hr)
u =K*A*u/Tp;                            % unit hydrograph flow rate (cfs)
u(u<0)=0;

rd=load(rdname);                        % dimensionless rainfall distribution
[m,n]=size(rd);
rmax=max(max(rd));
rmin=min(min(rd));
if m<2 | n~=2 | rmin<0 | rmax>1 | rd(1,:)~=[0 0] | rd(end,:)~=[1 1] %#ok (R11)
  msgbox('Rainfall Distribution is improperly formed.',...
    'File Error','error','modal');
  tq=[];q=[];tu=[];u=[];t=[];p=[];Q=[];code=0;
  return
end

T =D*rd(:,1);
P =R*rd(:,2);
% resample rainfall distribution
t =(0:d:T(end))';                       % rainfall time (hr)
p =interp1(T,P,t,method2);              % rainfall depth (in)

Q =zeros(size(t));
s =1000/CN-10;                          % retention (in)
i =find(p>Iar*s);
Q(i)=(p(i)-Iar*s).^2./(p(i)+(1-Iar)*s); % runoff depth (in)
dQ=[diff(Q);0];                         % incremental runoff depth (in)
q =conv(dQ,u);                          % runoff flow rate (cfs)
q(q<0)=0;
tq=(0:d:t(end)+tu(end))';               % runoff time (hr)

if units
  u=u/cm2cf; q=q/cm2cf;                 % cms
  p=p*25.4; Q=Q*25.4;                   % mm
end


function [V,D,bnd,j,work] = laneig(A,nin,k,sigma,options)

%LANEIG  Compute a few eigenvalues and eigenvectors.
%   LANEIG solves the eigenvalue problem A*v=lambda*v, when A is
%   real and symmetric using the Lanczos algorithm with partial
%   reorthogonalization (PRO).
%
%   [V,D] = LANEIG(A)
%   [V,D] = LANEIG('Afun',N)
%
%   The first input argument is either a real symmetric matrix, or a
%   string containing the name of an M-file which applies a linear
%   operator to the columns of a given matrix.  In the latter case,
%   the second input argument must be N, the order of the problem.
%
%   The full calling sequence is
%
%   [V,D,ERR] = LANEIG(A,K,SIGMA,OPTIONS)
%   [V,D,ERR] = LANEIG('Afun',N,K,SIGMA,OPTIONS)
%
%   On exit ERR contains the computed error bounds.  K is the number of
%   eigenvalues desired and SIGMA is numerical shift or a two letter string
%   which specifies which part of the spectrum should be computed:
%
%   SIGMA            Specified eigenvalues
%
%   'AL'            Algebraically Largest
%   'AS'            Algebraically Smallest
%   'LM'            Largest Magnitude   (default)
%   'SM'            Smallest Magnitude  (does not work when A is an m-file)
%   'BE'            Both Ends.  Computes k/2 eigenvalues
%                   from each end of the spectrum (one more
%                   from the high end if k is odd.)
%
%   The OPTIONS structure specifies certain parameters in the algorithm.
%
%    Field name      Parameter                              Default
%
%    OPTIONS.tol     Convergence tolerance                  16*eps
%    OPTIONS.lanmax  Dimension of the Lanczos basis.
%    OPTIONS.v0      Starting vector for the Lanczos        rand(n,1)-0.5
%                    iteration.
%    OPTIONS.delta   Level of orthogonality among the       sqrt(eps/K)
%                    Lanczos vectors.
%    OPTIONS.eta     Level of orthogonality after           10*eps^(3/4)
%                    reorthogonalization.
%    OPTIONS.cgs     reorthogonalization method used        0
%                    '0' : iterated modified Gram-Schmidt
%                    '1' : iterated classical Gram-Schmidt
%    OPTIONS.elr     If equal to 1 then extended local      1
%                    reorthogonalization is enforced.
%
%   See also LANPRO, EIGS, EIG.

% References:
% R.M. Larsen, Ph.D. Thesis, Aarhus University, 1998.
%
% B. N. Parlett, ``The Symmetric Eigenvalue Problem'',
% Prentice-Hall, Englewood Cliffs, NJ, 1980.
%
% H. D. Simon, ``The Lanczos algorithm with partial reorthogonalization'',
% Math. Comp. 42 (1984), no. 165, 115--142.

% Rasmus Munk Larsen, DAIMI, 1998

%%%%%%%%%%%%%%%%%%%%% Parse and check input arguments. %%%%%%%%%%%%%%%%%%%%%%

if ~isstr(A)
  if nargin<1
    error('Not enough input arguments.');
  end
  [m n] = size(A);
  Aisfunc = 0;
  if m~=n | ~isequal(A,A') | ~isreal(A)
    error('A must be real symmetric')
  end
  if nargin < 4 | isempty(sigma)
    options = [];
  else
    options = sigma;
  end
  if nargin < 3 | isempty(k), sigma = 'LM'; else, sigma = k; end
  if nargin < 2 | isempty(nin), k = min(n,5); else, k = nin; end
else
  if nargin<2
    error('Not enough input arguments.');
  end
  Aisfunc = 1;
  n = nin;
  if nargin < 5 | isempty(options)
    options.tol = 16*eps;
    options.lanmax = n;
    options.v0 = rand(n,1)-0.5;
  end
  if nargin < 4 | isempty(sigma), sigma = 'LM'; end
  if nargin < 3 | isempty(k), k = min(n,5);  end
end

if ~isnumeric(k) | real(abs(fix(k)))~=k | ~isnumeric(n) | real(abs(fix(n)))~=n
  error('Input arguments N and K must be positive integers.')
end

% Quick return for n<2  or k<1
if n < 1 | k<1
  if nargout < 2
    V = zeros(k,1);
  else
    V = eye(n,k);
    D = zeros(k,k);
    bnd =zeros(k,1);
  end
  return
end
if n == 1
  if ~Aisfunc
    D = A;
    V = 1;
    bnd = 0;
  else
    D = feval(A,1);
    V = 1;
    dnb = 0;
  end
  if nargout<2
    V=D;
  end
  return
end

% A is the matrix of all zeros (not detectable if A is a string)
if ~Aisfunc
  if nnz(A)==0
    if nargout < 2
      V = zeros(k,1);
    else
      V = eye(n,k);
      D = zeros(k,k);
      bnd =zeros(k,1);
    end
    return
  end
end

lanmax = n;
tol = 16*eps;
r = rand(n,1)-0.5;
part = sigma;
% Parse options struct
if ~isempty(options) & isstruct(options)
  c = fieldnames(options);
  for i=1:length(c)
    if strmatch(c(i),'v0'), r = getfield(options,'v0'); r=r(:); end
    if strmatch(c(i),'tol'), tol = getfield(options,'tol'); end
    if strmatch(c(i),'lanmax'), lanmax = getfield(options,'lanmax'); end
  end
end

% Protect against absurd arguments.
tol = max(tol,eps);
lanmax = min(lanmax,n);
if size(r,1)~=n
  error('v0 must be a vector of length n')
end

lanmax = min(lanmax,n);
if k>lanmax
  error('K must satisfy  K <= LANMAX <= N.');
end
ksave = k;

if strcmp(sigma,'SM') & ~isstr(A)
  sigma = 0;
end

% Prepare for shift-and-invert if sigma is numeric.
if  isnumeric(sigma)
  part = 'LM';
  if isstr(A)
    error('Shift-and-invert works only when the matrix A is given explicitly.');
  else
    pmmd = symmmd(A);
    A = A(pmmd,pmmd);
    [S.L,S.U] = lu(A - sigma*speye(n));
    condU = condest(S.U);
    dsigma = n * full(max(max(abs(A)))) * eps;
    if sigma < 0
      sgnsig = -1;
    else
      sgnsig = 1;
    end
    sigitr = 1;
    while condU > 1/eps & ((dsigma <= 1 & sigitr <= 10) | ~isfinite(condU))
      disps1 = sprintf(['sigma = %10e is near an exact eigenvalue of A,\n' ...
			'so we cannot use the LU factorization of (A-sigma*I): ' ...
			' condest(U) = %10e.\n'],sigma,condU);
      if abs(sigma) < 1
	sigma = sigma + sgnsig * dsigma;
	disps2 = sprintf('We are trying sigma + %10e = %10e instead.\n', ...
			 sgnsig*dsigma,sigma);
      else
	sigma = sigma * (1 + dsigma);
	disps2 = sprintf('We are trying sigma * (1 + %10e) = %10e instead.\n', ...
			 dsigma,sigma);
      end
      %     if nargout < 3 & dispn ~= 0
      disp([disps1 disps2])
      %     end
      [S.L,S.U] = lu(A - sigma*speye(n));
      condU = condest(S.U);
      dsigma = 10 * dsigma;
      sigitr = sigitr + 1;
    end
  end
  A = S;
end

neig = 0; nrestart=-1;
if ~strcmp(part,'BE')
  j = min(2*k+2,lanmax);
else
  j = min(k+1,lanmax);
end

%%%%%%%%%%%%%%%%%%%%% Here begins the computation  %%%%%%%%%%%%%%%%%%%%%%

V = []; T = []; anorm = []; work = zeros(1,2); rnorm=-1;

while neig < k
  %%%%%%%%%%%%%%%%%%%%% Compute Lanczos tridiagonalization %%%%%%%%%%%%%%%%%
  j = min(lanmax,j+1-mod(j,2));
  % "Trick" to avoid unwanted zero eigenvalues when laneig is used for
  % SVD calculations. (Nothing to if lanmax is odd, though.)

  if  ~isstr(A)
    [V,T,r,anorm,ierr,w] = lanpro(A,j,r,options,V,T,anorm);
  else
    [V,T,r,anorm,ierr,w] = lanpro(A,n,j,r,options,V,T,anorm);
  end
  work= work + w;

  if ierr<0 % Invariant subspace of dimension -ierr found.
    j = -ierr;
  end

  %%%%%%%%%%%%%%%%%% Compute eigenvalues and error bounds %%%%%%%%%%%%%%%%%%
  % Analyze T
  [D,top,bot,err] = tqlb([full(diag(T))],full([0;diag(T,1)]));
  %  if err>0
  %    printf(['TQLB failed. Eigenvalue no. %i did not converge in 30', ...
  %	  ' iterations'],err);
  %  end
  %  full(T)
  %  [P,D] = eig(full(T));
  %  D = diag(D);
  %  bot = P(end,:)';
  %  [P(1,:)' P(end,:)']
  [D,I] = sort(D);
  bot = bot(I);

  % Set simple error bounds
  rnorm = norm(r);
  bnd = rnorm*abs(bot);

  % Use Largest Ritz value to estimate ||A||_2. This might save some
  % reorth. in case of restart.
  anorm = max(abs(D));

  % Estimate gap structure and refine error bounds
  bnd = refinebounds(D,bnd,n*eps*anorm);

  %%%%%%%%%%%%%%%%%%% Check convergence criterion %%%%%%%%%%%%%%%%%%%%
  % Reorder eigenvalues according to SIGMA
  switch part
   case 'AS'
    IPART = 1:j;
   case 'AL'
    IPART = j:-1:1;
   case 'LM'
    [dummy,IPART] = sort(-abs(D));
   case 'BE'
    if j<k
      IPART=1:j;
    else
      mid = floor(k/2);
      par = rem(k,1);
      IPART = [1:mid,(j-mid-par):j]';
    end
   otherwise
    error(['Illegal value for SIGMA: ',part]);
  end
  D = D(IPART);  bnd = bnd(IPART);
  if isnumeric(sigma)
    D = sigma + 1./D;
  end

  % Check if enough have converged.
  neig = 0;
  for i=1:min(j,k)
    if bnd(i) <= tol*abs(D(i))
      neig = neig + 1;
    end
  end

  %%%%%%%%%%% Check whether to stop or to extend the Krylov basis? %%%%%%%%%%
  if ierr<0 % Invariant subspace found
    if j<k
      warning(['Invariant subspace of dimension ',num2str(j-1),' found.'])
    end
    break;
  end
  if j>=lanmax % Maximal dimension of Krylov subspace reached => Bail out!
    if neig<ksave
      warning(['Maximum dimension of Krylov subspace exceeded prior',...
	       ' to convergence.']);
    end
    break;
  end

  % Increase dimension of Krylov subspace and try again.
  if neig>0
    %    j = j + ceil(min(20,max(2,((j-1)*(k-neig+1))/(2*(neig+1)))));
    j = j + min(100,max(2,0.5*(k-neig)*j/(neig+1)));
  elseif neig<k
    %    j = j + ceil(min(20,max(8,(k-neig)/2)));
    j = max(1.5*j,j+10);
  end
  j = min(j+1,lanmax);
  nrestart = nrestart + 1;
end

%%%%%%%%%%%%%%%% Lanczos converged (or failed). Prepare output %%%%%%%%%%%%%%%
k = min(ksave,j);

if nargout>1
  j = size(T,1);
  [Q,D] = eig(full(T)); D = diag(D);
  [D,I] = sort(D);
  % Compute and normalize Ritz vectors (overwrite V to save memory).
  V = V*Q(:,I(IPART(1:k)));
  for i=1:k
    nq = norm(V(:,i));
    if isfinite(nq) & nq~=0 & nq~=1
      V(:,i) = V(:,i)/nq;
    end
  end
  [D,I] = sort(D);
  D = D(IPART(1:k));
  if isnumeric(sigma)
    D = sigma + 1./D;
    V(pmmd,:) = V;
  end
end

% Pick out desired part of the spectrum
if length(D)~=k
  D = D(1:k);
  bnd = bnd(1:k);
end

if nargout<2
  V = D;
else
  D = diag(D);
end


function T = logm(q,varargin)
% the logarithmic map that translates a rotation into a spin tensor
%
% Syntax
%   T = logm(q) % spin tensor with reference to the identical rotation
%   T = logm(q,q_ref) % spin tensor with reference q_ref
%
% Input
%  q - @quaternion
%  q_ref - @quaternion
%
% Output
%  T - @spinTensor
%
% See also
% spinTensor/exp

tq = log(q,varargin{:});

M = zeros([3,3,size(q)]);

M(2,1,:) =  tq.z;
M(3,1,:) = -tq.y;
M(3,2,:) =  tq.x;

M(1,2,:) = -tq.z;
M(1,3,:) =  tq.y;
M(2,3,:) = -tq.x;

% make it a spinTensor
T = spinTensor(M);


function incidence_matrix = gr_incidence_matrix ( node_num, ...
  node_coordinates, edge_num, edge_nodes )

%*****************************************************************************80
%
%% GR_INCIDENCE_MATRIX computes the incidence matrix.
%
%  Discussion:
%
%    The incidence matrix is of order EDGE_NUM by NODE_NUM.
%
%    A(I,J) = 1 if edge I uses node J.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_COORDINATES(2,NODE_NUM), the coordinates of the nodes.
%
%    Input, integer EDGE_NUM, the number of edges.
%
%    Input, integer EDGE_NODES(2,EDGE_NUM), the indices of the two nodes
%    that form each edge.
%
%    Output, integer INCIDENCE_MATRIX(EDGE_NUM,NODE_NUM), the incidence
%    matrix.
%
  incidence_matrix = zeros ( edge_num, node_num );

  for e = 1 : edge_num
    for i = 1 : 2
      n = edge_nodes(i,e);
      incidence_matrix(e,n) = incidence_matrix(e,n) + 1;
    end
  end

  return
end


function triangulation_test08 ( )

%*****************************************************************************80
%
%% TEST08 tests R8TRIS2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 June 2005
%
%  Author:
%
%    John Burkardt
%
  node_num = 9;
  dim_num = 2;

  node_xy = [ ...
       0.0, 0.0; ...
       0.0, 1.0; ...
       0.2, 0.5; ...
       0.3, 0.6; ...
       0.4, 0.5; ...
       0.6, 0.4; ...
       0.6, 0.5; ...
       1.0, 0.0; ...
       1.0, 1.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST08\n' );
  fprintf ( 1, '  R8TRIS2 computes the Delaunay triangulation of\n' );
  fprintf ( 1, '    a set of nodes in 2D.\n' );
%
%  Set up the Delaunay triangulation.
%
  [ triangle_num, triangle_node, triangle_neighbor ] = r8tris2 ( ...
    node_num, node_xy );

  triangulation_order3_print ( node_num, triangle_num, node_xy, ...
    triangle_node, triangle_neighbor );

  return
end


function [Signal, Sigma] = MPdenoising(data, mask, kernel, sampling, centering)
    %
    % "MPPCA": 4d image denoising and noise map estimation by exploiting  data redundancy in the PCA domain using universal properties of the eigenspectrum of
    % random covariance matrices, i.e. Marchenko Pastur distribution
    %
    %  [Signal, Sigma] = MPdenoising(data, mask, kernel, sampling)
    %       output:
    %           - Signal: [x, y, z, M] denoised data matrix
    %           - Sigma: [x, y, z] noise map
    %       input:
    %           - data: [x, y, z, M] data matrix
    %           - mask:   (optional)  region-of-interest [boolean]
    %           - kernel: (optional)  window size, typically in order of [5 x 5 x 5]
    %           - sampling:
    %                    1. full: sliding window (default for noise map estimation, i.e. [Signal, Sigma] = MPdenoising(...) )
    %                    2. fast: block processing (default for denoising, i.e. [Signal] = MPdenoising(...))
    %
    %  Authors: Jelle Veraart (jelle.veraart@nyumc.org)
    % Copyright (c) 2016 New York Universit and University of Antwerp
    %
    %      Permission is hereby granted, free of charge, to any non-commercial entity
    %      ('Recipient') obtaining a copy of this software and associated
    %      documentation files (the 'Software'), to the Software solely for
    %      non-commercial research, including the rights to use, copy and modify the
    %      Software, subject to the following conditions:
    %
    %        1. The above copyright notice and this permission notice shall be
    %      included by Recipient in all copies or substantial portions of the
    %      Software.
    %
    %        2. THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND,
    %      EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIESOF
    %      MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN
    %      NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BELIABLE FOR ANY CLAIM,
    %      DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
    %      OTHERWISE, ARISING FROM, OUT OF ORIN CONNECTION WITH THE SOFTWARE OR THE
    %      USE OR OTHER DEALINGS IN THE SOFTWARE.
    %
    %        3. In no event shall NYU be liable for direct, indirect, special,
    %      incidental or consequential damages in connection with the Software.
    %      Recipient will defend, indemnify and hold NYU harmless from any claims or
    %      liability resulting from the use of the Software by recipient.
    %
    %        4. Neither anything contained herein nor the delivery of the Software to
    %      recipient shall be deemed to grant the Recipient any right or licenses
    %      under any patents or patent application owned by NYU.
    %
    %        5. The Software may only be used for non-commercial research and may not
    %      be used for clinical care.
    %
    %        6. Any publication by Recipient of research involving the Software shall
    %      cite the references listed below.
    %
    % REFERENCES
    %      Veraart, J.; Fieremans, E. & Novikov, D.S. Diffusion MRI noise mapping
    %      using random matrix theory Magn. Res. Med., 2016, early view, doi:
    %      10.1002/mrm.26059

    if isa(data,'integer')
        data = single(data);
    end
    [sx, sy, sz, M] = size(data);

    if ~exist('mask', 'var') || isempty(mask)
        mask = true([sx, sy, sz]);
    end
    if ~isa(mask,'boolean')
        mask = mask>0;
    end

    if ~exist('kernel', 'var') || isempty(kernel)
        kernel = [5 5 5];
    end

    if isscalar(kernel)
        kernel = [kernel, kernel, kernel];
    end
    kernel = kernel + (mod(kernel, 2)-1);   % needs to be odd.
    k = (kernel-1)/2; kx = k(1); ky = k(2); kz = k(3);
    N = prod(kernel);

    if ~exist('sampling', 'var') || isempty(sampling)
        if nargout > 1
            sampling = 'full';
        else
            sampling = 'fast';
        end
    end

    % create mask
    if ~exist('mask', 'var') || isempty(mask)
        mask = true(sx, sy, sz);
    end

    if ~exist('centering', 'var') || isempty(centering)
        centering = false;
    end

    if strcmp(sampling, 'fast')
        if nargout>1
            warning('undersampled noise map will be returned')
        end
        % compute center points of patches
        stats = regionprops(mask, 'BoundingBox');
        n = ceil(stats.BoundingBox(4:6) ./ kernel);

        x = linspace(ceil(stats.BoundingBox(1))+k(1), floor(stats.BoundingBox(1))-k(1) + stats.BoundingBox(4), n(1)); x = round(x);
        y = linspace(ceil(stats.BoundingBox(2))+k(2), floor(stats.BoundingBox(2))-k(2) + stats.BoundingBox(5), n(2)); y = round(y);
        z = linspace(ceil(stats.BoundingBox(3))+k(3), floor(stats.BoundingBox(3))-k(3) + stats.BoundingBox(6), n(3)); z = round(z);

        [y, x, z] = meshgrid(x, y, z); x = x(:); y = y(:); z = z(:);
    end

    if strcmp(sampling, 'full')
        warning('image boundaries are not processed.')
        mask(1:k(1), :, :) = 0;
        mask(sx-k(1):sx, :, :) = 0;

        mask(:, 1:k(2), :) = 0;
        mask(:, sy-k(2):sy, :, :) = 0;
        mask(:,:,1:k(3)) = 0;
        mask(:,:,sz-k(3)) = 0;

        x = []; y = []; z = [];
        for i = k(3)+1:sz-k(3)
            [x_, y_] = find(mask(:,:,i) == 1);
            x = [x; x_]; y = [y; y_];  z = [z; i*ones(size(y_))];
        end
        x = x(:); y = y(:); z = z(:);
    end

    % Declare variables:
    if logical(exist('OCTAVE_VERSION', 'builtin')) % for Octave, zeros(..,'like',data) not implemented. replace with class(data).
        zerosoption = {class(data)};
    else
        zerosoption = {'like', data};
    end
    sigma = zeros(1, numel(x), zerosoption{:});
    npars = zeros(1, numel(x), zerosoption{:});
    signal = zeros(M, prod(kernel), numel(x), zerosoption{:});

    Sigma = zeros(sx, sy, sz, zerosoption{:});
    Npars = zeros(sx, sy, sz, zerosoption{:});
    Signal = zeros(sx, sy, sz, M, zerosoption{:});

    % compute scaling factor for in case N<M
    R = min(M, N);
    scaling = (max(M, N) - (0:R-centering-1)) / N;
    scaling = scaling(:);

    % start denoising
    for nn = 1:numel(x)

        % create data matrix
        X = data(x(nn)-kx:x(nn)+kx, y(nn)-ky:y(nn)+ky, z(nn)-kz:z(nn)+kz, :);
        X = reshape(X, N, M); X = X';

        if centering
            colmean = mean(X, 1);
            X = X - repmat(colmean, [M, 1]);
        end
        % compute PCA eigenvalues
        [u, vals, v] = svd(X, 'econ');
        vals = diag(vals).^2 / N;

        % First estimation of Sigma^2;  Eq 1 from ISMRM presentation
        csum = cumsum(vals(R-centering:-1:1)); cmean = csum(R-centering:-1:1)./(R-centering:-1:1)'; sigmasq_1 = cmean./scaling;

        % Second estimation of Sigma^2; Eq 2 from ISMRM presentation
        gamma = (M - (0:R-centering-1)) / N;
        rangeMP = 4*sqrt(gamma(:));
        rangeData = vals(1:R-centering) - vals(R-centering);
        sigmasq_2 = rangeData./rangeMP;

        % sigmasq_2 > sigma_sq1 if signal-components are represented in the
        % eigenvalues

        t = find(sigmasq_2 < sigmasq_1, 1);

        if isempty(t)
            sigma(nn) = NaN;
            signal(:, :, nn) = X;
            t = R+1;
        else
            sigma(nn) = sqrt(sigmasq_1(t));
            vals(t:R) = 0;
            s = u*diag(sqrt(N*vals))*v';
            if centering
               s = s + repmat(colmean, [M, 1]);
            end

            signal(:, :, nn) = s;
        end
        npars(nn) = t-1;
    end

    for nn = 1:numel(x)
        Sigma(x(nn), y(nn), z(nn)) = sigma(nn);
        Npars(x(nn), y(nn), z(nn)) = npars(nn);
        if strcmp(sampling, 'fast')
            Signal(x(nn)-k(1):x(nn)+k(1),y(nn)-k(2):y(nn)+k(2),z(nn)-k(3):z(nn)+k(3), :) = unpatch(signal(:,:,nn), k);
        elseif strcmp(sampling, 'full')
            Signal(x(nn), y(nn),z(nn), :) = signal(:,ceil(prod(kernel)/ 2),nn);
        end
    end
end

function data = unpatch(X, k)
    kernel=k+k+1;
    data = zeros([kernel, size(X, 1)]);
    tmp = zeros(kernel);
    for i = 1:size(X, 1);
        tmp(:) = X(i, :);
        data(:,:,:,i) = tmp;
    end
end


function [RR_e_resamp,QRS_A_resamp] = clean_and_resample_intervals(onsets,QRS_A1,Fs)

% Use same pre-processing used for ECG in the paper ''
%
%

IBI = diff(onsets)./Fs;
t_IBI = onsets(2:end)./Fs;

RR_e0 = IBI;     % no ectopic rejection
QRS_A0 = QRS_A1;

% reject ectopic beats
% remove invalid R-R intervals if RR<0.3 or RR>2.0 & 20% outside of 41 points moving mean exclude

RR_mean_41 = meanfilt1(IBI,41);
RR = [];    % RR interval in seconds
RR_time=[]; % time of RR interval in seconds
v_RR = ones(1,length(IBI))*-1;
n_RR = 0;
for j=1:length(IBI)
    if abs(IBI(j) - RR_mean_41(j)) < 0.2*RR_mean_41(j) &&  IBI(j)>= 0.3 && IBI(j) <= 2.0
        n_RR = n_RR+1;
        RR(n_RR) = IBI(j);
        RR_time(n_RR) = onsets(j)/Fs;
        v_RR(j)=1;
    end
end
valid_RR=find(v_RR==1);
% remove invalid QRS peaks if 50% outside of 41 points moving mean
QRS_A_mean_41=meanfilt1(QRS_A1,41);
n_QRS=0;
QRS=[];
QRS_time=[];
v_QRS=ones(1,length(QRS_A1))*-1;
for k=1:length(QRS_A1)
    if abs(QRS_A1(k)-QRS_A_mean_41(k))<0.5*QRS_A_mean_41(k)
        n_QRS=n_QRS+1;
        QRS(n_QRS) = QRS_A1(k);
        QRS_time(n_QRS) = tIBI(j);
        v_QRS(k)=1;
    end
end
valid_QRS=find(v_QRS==1);
valid_RRQRS=intersect(valid_RR,valid_QRS);
valid_RR=valid_RRQRS;

onsets = onsets(valid_RR);
QRS_A1=QRS_A1(valid_RR);

RR_e1 = IBI(valid_RR);

% resample to 4Hz
onsets = onsets./Fs;
t2=round(onsets(end));
Fs_resamp=4;
t1=(1/Fs_resamp):(1/Fs_resamp):t2; % 4Hz resample

% linear interpolation
QRS_A_resamp = interp1(onsets,QRS_A1,t1);%,'spline');
RR_e_resamp = interp1(onsets,RR_e1,t1);%,'spline');
QRS_A_resamp(find(isnan(QRS_A_resamp)))=nanmean(QRS_A_resamp);
RR_e_resamp(find(isnan(RR_e_resamp)))=nanmean(RR_e_resamp);


function [Hx,Hy,Ez,time] = MaxwellPNonCon2D(pinfo, Hx, Hy, Ez, FinalTime)

% function [Hx,Hy,Ez] = MaxwellPNonCon2D(pinfo, Hx, Hy, Ez, FinalTime)
% Purpose  : Integrate TM-mode Maxwell's until FinalTime starting with initial conditions Hx,Hy,Ez

Globals2D;
time = 0;

% Runge-Kutta residual storage
resHx = zeros(size(Hx)); resHy = resHx; resEz = resHx;

% compute time step size (taking into account variable polynomial order)
dt = 100;
for N=1:length(pinfo)
  dt = min(dt, 2./( N^2*max(pinfo(N).Fscale(:)/2)));
end

% outer time step loop
tstep = 1;
while (time<FinalTime)

   if(time+dt>FinalTime), dt = FinalTime-time; end

   for INTRK = 1:5
      % compute right hand side of TM-mode Maxwell's equations
      [rhsHx, rhsHy, rhsEz] = MaxwellPNonConRHS2D(pinfo, Hx,Hy,Ez);

      % initiate and increment Runge-Kutta residuals
      resHx = rk4a(INTRK)*resHx + dt*rhsHx;
      resHy = rk4a(INTRK)*resHy + dt*rhsHy;
      resEz = rk4a(INTRK)*resEz + dt*rhsEz;

      % update fields
      Hx = Hx+rk4b(INTRK)*resHx;
      Hy = Hy+rk4b(INTRK)*resHy;
      Ez = Ez+rk4b(INTRK)*resEz;
   end;

   % Increment time
   time = time+dt; tstep = tstep+1;
end
return


classdef RMMEDA_F4 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing RM-MEDA

%------------------------------- Reference --------------------------------
% Q. Zhang, A. Zhou, and Y. Jin, RM-MEDA: A regularity model-based
% multiobjective estimation of distribution algorithm, IEEE Transactions on
% Evolutionary Computation, 2008, 12(1): 41-63.
%------------------------------- 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
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 3;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            g = sum((X(:,3:end)-repmat(X(:,1),1,size(X,2)-2)).^2,2);
            PopObj(:,1) = cos(pi/2*X(:,1)).*cos(pi/2*X(:,2)).*(1+g);
            PopObj(:,2) = cos(pi/2*X(:,1)).*sin(pi/2*X(:,2)).*(1+g);
            PopObj(:,3) = sin(pi/2*X(:,1)).*(1+g);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,3);
            R = R./repmat(sqrt(sum(R.^2,2)),1,3);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            a = linspace(0,pi/2,10)';
            R = {sin(a)*cos(a'),sin(a)*sin(a'),cos(a)*ones(size(a'))};
        end
    end
end


function out = CO_Embed2_AngleTau(y,maxTau)
% CO_Embed2_AngleTau Angle autocorrelation in a 2-dimensional embedding space
%
% Investigates how the autocorrelation of angles between successive points in
% the two-dimensional time-series embedding change as tau varies from
% tau = 1, 2, ..., maxTau.
%
%---INPUTS:
% y, a column vector time series
% maxTau, the maximum time lag to consider

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

doPlot = false;
tauRange = (1:1:maxTau);
numTau = length(tauRange);

% Ensure y is a column vector
if size(y,2) > size(y,1);
	y = y';
end

stats_store = zeros(3,numTau);

for i = 1:numTau
	tau = tauRange(i);

	m = [y(1:end-tau), y(1+tau:end)];

	theta = diff(m(:,2))./diff(m(:,1));
	theta = atan(theta); % measured as deviation from the horizontal

	if isempty(theta)
		error('Time series (N=%u) too short for embedding',length(y));
	end

	stats_store(1,i) = CO_AutoCorr(theta,1,'Fourier');
	stats_store(2,i) = CO_AutoCorr(theta,2,'Fourier');
	stats_store(3,i) = CO_AutoCorr(theta,3,'Fourier');
end

if doPlot
    figure('color','w'); box('on');
    plot(stats_store');
end

% ------------------------------------------------------------------------------
% Compute lots of outputs statistics:
% ------------------------------------------------------------------------------
out.ac1_thetaac1 = CO_AutoCorr(stats_store(1,:),1,'Fourier');
out.ac1_thetaac2 = CO_AutoCorr(stats_store(2,:),1,'Fourier');
out.ac1_thetaac3 = CO_AutoCorr(stats_store(3,:),1,'Fourier');
out.mean_thetaac1 = mean(stats_store(1,:));
out.max_thetaac1 = max(stats_store(1,:));
out.min_thetaac1 = min(stats_store(1,:));
out.mean_thetaac2 = mean(stats_store(2,:));
out.max_thetaac2 = max(stats_store(2,:));
out.min_thetaac2 = min(stats_store(2,:));
out.mean_thetaac3 = mean(stats_store(3,:));
out.max_thetaac3 = max(stats_store(3,:));
out.min_thetaac3 = min(stats_store(3,:));
out.meanrat_thetaac12 = out.mean_thetaac1/out.mean_thetaac2;
out.diff_thetaac12 = sum(abs(stats_store(2,:)-stats_store(1,:)));

end


function [disp_row, disp_col] = resp_newton(response, responsef, iterations, ky, kx, use_sz)

[max_resp_row, max_row] = max(response, [], 1);
[init_max_response, max_col] = max(max_resp_row, [], 2);
max_row_perm = permute(max_row, [2 3 1]);
col = max_col(:)';
row = max_row_perm(sub2ind(size(max_row_perm), col, 1:size(response,3)));

trans_row = mod(row - 1 + floor((use_sz(1)-1)/2), use_sz(1)) - floor((use_sz(1)-1)/2);
trans_col = mod(col - 1 + floor((use_sz(2)-1)/2), use_sz(2)) - floor((use_sz(2)-1)/2);
init_pos_y = permute(2*pi * trans_row / use_sz(1), [1 3 2]);
init_pos_x = permute(2*pi * trans_col / use_sz(2), [1 3 2]);
max_pos_y = init_pos_y;
max_pos_x = init_pos_x;

% pre-compute complex exponential
exp_iky = exp(bsxfun(@times, 1i * ky, max_pos_y));
exp_ikx = exp(bsxfun(@times, 1i * kx, max_pos_x));

% gradient_step_size = gradient_step_size / prod(use_sz);

ky2 = ky.*ky;
kx2 = kx.*kx;

iter = 1;
while iter <= iterations
    % Compute gradient
    ky_exp_ky = bsxfun(@times, ky, exp_iky);
    kx_exp_kx = bsxfun(@times, kx, exp_ikx);
    y_resp = mtimesx(exp_iky, responsef, 'speed');
    resp_x = mtimesx(responsef, exp_ikx, 'speed');
    grad_y = -imag(mtimesx(ky_exp_ky, resp_x, 'speed'));
    grad_x = -imag(mtimesx(y_resp, kx_exp_kx, 'speed'));
    ival = 1i * mtimesx(exp_iky, resp_x, 'speed');
    H_yy = real(-mtimesx(bsxfun(@times, ky2, exp_iky), resp_x, 'speed') + ival);
    H_xx = real(-mtimesx(y_resp, bsxfun(@times, kx2, exp_ikx), 'speed') + ival);
    H_xy = real(-mtimesx(ky_exp_ky, mtimesx(responsef, kx_exp_kx, 'speed'), 'speed'));
    det_H = H_yy .* H_xx - H_xy .* H_xy;

    % Compute new position using newtons method
    max_pos_y = max_pos_y - (H_xx .* grad_y - H_xy .* grad_x) ./ det_H;
    max_pos_x = max_pos_x - (H_yy .* grad_x - H_xy .* grad_y) ./ det_H;

    % Evaluate maximum
    exp_iky = exp(bsxfun(@times, 1i * ky, max_pos_y));
    exp_ikx = exp(bsxfun(@times, 1i * kx, max_pos_x));

    iter = iter + 1;
end
max_response = 1 / prod(use_sz) * real(mtimesx(mtimesx(exp_iky, responsef, 'speed'), exp_ikx, 'speed'));

% check for scales that have not increased in score
ind = max_response < init_max_response;
max_pos_y(ind) = init_pos_y(ind);
max_pos_x(ind) = init_pos_x(ind);
disp_row = (mod(max_pos_y(1,1,1) + pi, 2*pi) - pi) / (2*pi) * use_sz(1);
disp_col = (mod(max_pos_x(1,1,1) + pi, 2*pi) - pi) / (2*pi) * use_sz(2);
end