function [Ka, pKa] = acidDissociationConstant(metAbbr, Alberty2006, metAbbrAlbertyAbbr, temp, is, chi)
% Acid dissociation constant for the different metabolite species that make up a reactant
%
% USAGE:
%
%    [Ka, pKa] = acidDissociationConstant(metAbbr, Alberty2006, metAbbrAlbertyAbbr, temp, is, chi)
%
% INPUTS:
%    metAbbr:               reconstruction reactant abbreviation
%    Alberty2006:           Basic data on the metabolite species that make
%                           up a reactant, compiled by Robert A. Alberty,
%                           Massachusetts Institute of Technology.
%                           In Print: `Robert A. Alberty, Biochemical Thermodynamics:
%                           Applications of Mathematica. John Wiley & Sons, 2006. p391-395`
%                           Online: BasicBioChemData3.nb
%                           http://library.wolfram.com/infocenter/MathSource/5704/
%    metAbbrAlbertyAbbr:    mapping from model metabolite primary key to
%                           primary key of reactants in `Alberty2006`
%
% OPTIONAL INPUT
%    temp:                  temperature (default 298.15 K)
%    is:                    ionic strength (default 0 M)
%    chi:                   electrical potential (default 0)
%
% OUTPUTS:
%    Ka:                    apparent equilibrium constants
%    pKa:                   :math:`-log_{10}(Ka)`

if ~exist('temp','var')
    temp=298.15;
end
if ~exist('is','var')
    is=0;
end
if ~exist('chi','var')
    chi=0;
end

%find the alberty abbreviation for this metabolite Abbreviation
albertyAbbr=metAbbrAlbertyAbbr(strcmp(metAbbr,metAbbrAlbertyAbbr(:,2)),3);

%make a list of alberty abbreviations
klt=size(Alberty2006,2);
allAlbertyAbbr=cell(klt,1);
for k=1:klt
    allAlbertyAbbr{k}=Alberty2006(k).abbreviation;
end
%index for matching data for the alberty abbreviation
n=find(strcmp(albertyAbbr,allAlbertyAbbr));

%find the number of species within pseudoisomer group
p=max(find(~isnan(Alberty2006(n).basicData(:,1))));

%no Legendre transformation for pH or electrical potential
Legendre     = 0;
LegendreCHI  = 0;
pHr = 7; %dummy, has no effect

[dGf0,dHf0,mf,aveHbound,aveZi,lambda,gpfnsp]=calcdGHT(Alberty2006(n).basicData(1:p,1),Alberty2006(n).basicData(1:p,2),Alberty2006(n).basicData(1:p,3),Alberty2006(n).basicData(1:p,4),pHr,is,temp,chi,Legendre,LegendreCHI);

Ka=zeros(p-1);
pKa=zeros(p-1);

gasConstant = 8.314472/1000; % kJ K-1 mol-1

%RTalpha p 49 Alberty 2003
%where alpha is the Debye-Huckel Constant
gibbscoeff = (9.20483*temp)/10^3 - (1.284668*temp^2)/10^5 + (4.95199*temp^3)/10^8;

% A <--> A- + H+
% Take into account the ionic strength effect on the Gibbs energy of a
% hydrogen ion
zi=1;
hydrogenIonistermG = -(gibbscoeff*(zi.^2)*is^0.5)/(1 + 1.6*is^0.5);

for x=1:p-1

    Ka(x)  =  exp((gpfnsp(x+1,1)-gpfnsp(x,1)-hydrogenIonistermG)/(gasConstant*temp));
    if 1
        pKa(x) =  -(gpfnsp(x+1,1)-gpfnsp(x,1)-hydrogenIonistermG)/(gasConstant*temp*log(10));
    else
        %p244 Alberty 2003 uses a slightly less accurate gas constant so slight
        %difference
        gpfnsp=Alberty2006(n).basicData(1:p,1);
        pKa(x) =  -(gpfnsp(x+1,1)-gpfnsp(x,1))/(log(10)* 8.31451 * .29815);
    end
end


function y = sample_node(CPD, pev)
% SAMPLE_NODE Draw a random sample from P(Xi | x(pi_i), theta_i)  (gaussian)
% y = sample_node(CPD, parent_evidence)
%
% pev{i} is the value of the i'th parent (if there are any parents)
% y is the sampled value (a scalar or vector)

if length(CPD.dps)==0
  i = 1;
else
  dpvals = cat(1, pev{CPD.dps});
  i = subv2ind(CPD.sizes(CPD.dps), dpvals(:)');
end

if length(CPD.cps) == 0
  y = gsamp(CPD.mean(:,i), CPD.cov(:,:,i), 1);
else
  pev = pev(:);
  x = cat(1, pev{CPD.cps});
  y = gsamp(CPD.mean(:,i) + CPD.weights(:,:,i)*x(:), CPD.cov(:,:,i), 1);
end
y = y(:);


function T = compute_tensor_field_random(n,options)

% compute_tensor_field_random - create 2D TF
%
%   T = compute_tensor_field_random(n,options);
%
%   Copyright (c) 2008 Gabriel Peyre

options.null = 0;
sigma_flow = getoptions(options, 'sigma_tensor', 50*n/256);
niter_tensor = getoptions(options, 'niter_tensor', 8);
verb = getoptions(options, 'verb', 1);

T = randn(n,n,3);
en = linspace(.5,1,n^2);
an = linspace(.1,1,n^2);
or = linspace(-pi/2,pi/2,n^2);
for i=1:niter_tensor
    if verb
        progressbar(i,niter_tensor);
    end
    T = perform_blurring(T,sigma_flow);
    U = perform_tensor_mapping(T,+1);
%    clf;
%   for k=1:3
%        subplot(1,3,k); a = U(:,:,k);
%        hist(a(:),100); axis tight;
%    end
%    drawnow;
    U(:,:,1) = perform_histogram_equalization(U(:,:,1), en);
    U(:,:,2) = perform_histogram_equalization(U(:,:,2), an);
    U(:,:,3) = perform_histogram_equalization(U(:,:,3), or);
    T = perform_tensor_mapping(U,-1);
end


function theta = get_theta_bounds(e1,e2,F,dim)

[theta1,~] = get_theta(e1,dim);
[theta2,l2] = get_theta(e2,dim);

%% trasfer l2 to i1
l2(:,1) = F_transfer_l(F,l2(:,1),e2);
l2(:,2) = F_transfer_l(F,l2(:,2),e2);

theta2(1) = l_get_angle(l2(:,1));
theta2(2) = l_get_angle(l2(:,2));

if theta2(1)>theta2(2)
    theta2 = flipud(theta2);
end

theta(1) = max(theta1(1),theta2(1));
theta(2) = min(theta1(2),theta2(2));


function x_stdft = stdft(x, N, K, N_fft)

frames      = 1:K:(length(x)-N);
x_stdft     = zeros(length(frames), N_fft);

w           = hanning(N);
x           = x(:);

for i = 1:length(frames)
    ii              = frames(i):(frames(i)+N-1);
	x_stdft(i, :) 	= fft(x(ii).*w, N_fft);
end


function pot = combine_pots(pot1, pot2)
% COMBINE_POTS combine two potentials
% pot = combine_pots(pot1, pot2)

% Reduce both potentials before trying to combine them.
% Cf. "Stable Local computation with Conditional Gaussian Distributions", page 9
% Consider again two potentials with minimal tail

% Guarantee minimal tails. If pot1 or pot2 are minimal, they are not changed
pot1 = reduce_pot(pot1);
pot2 = reduce_pot(pot2);

%if the intersect set of these two potentials' head conts. combination is undifined
if ~isempty( myintersect(pot1.cheaddom, pot2.cheaddom) )
    return;
end

if  isempty( myintersect(pot1.domain, pot2.cheaddom) ) | isempty( myintersect(pot2.domain, pot1.cheaddom))
    % if satisfy the condition of directed combine
    pot = direct_combine_pots(pot1, pot2);
else
    % perform recursive combine
    pot = recursive_combine_pots(pot1, pot2);
end


function compare_implementations()

n_trials = 50;
n_obstacles = 5;
matlab_times = nan(1, n_trials);
cpp_times = nan(1, n_trials);

iter = 1;
while iter <= n_trials
  dim = 1 + ceil(iter / 20);
  A_bounds = [eye(dim); -eye(dim)];
  b_bounds = [ones(dim, 1); zeros(dim, 1)];

  obstacle_pts = iris.test.random_obstacles(dim, n_obstacles, zeros(dim,1), ones(dim,1), 0.05);
  obstacles = mat2cell(obstacle_pts, size(obstacle_pts, 1), size(obstacle_pts, 2), ones(1, size(obstacle_pts, 3)));
  start = 0.5 * ones(dim,1);
  options = struct('require_containment', false,...
                   'error_on_infeasible_start', false,...
                   'termination_threshold', 2e-2,...
                   'iter_limit', 100);

  try
    t0 = tic();
    [A_c, b_c, C_c, d_c] = iris.inflate_regionmex(obstacles, A_bounds, b_bounds, start, options);
    cpp_times(iter) = toc(t0);

    t0 = tic();
    [A_m, b_m, C_m, d_m] = iris.inflate_region_fallback(obstacle_pts, A_bounds, b_bounds, start, options);
    matlab_times(iter) = toc(t0);

    iter = iter + 1
  catch
    continue
  end

  % assert(iris.util.equal_up_to_permutations(A_c, A_m, 1e-2));
  % assert(iris.util.equal_up_to_permutations(b_c, b_m, 1e-2));
  assert(all(all(abs(C_c - C_m) < 1e-2)));
  assert(all(all(abs(d_c - d_m) < 1e-2)));

end

fprintf(1, 'c++ mean: %f std: %f\n', mean(cpp_times), std(cpp_times));
fprintf(1, 'matlab mean: %f std: %f\n', mean(matlab_times), std(matlab_times));


function [ o, x, w ] = en_r2_11_1 ( n, option )

%*****************************************************************************80
%
%% EN_R2_11_1 implements the Stroud rule 11.1 for region EN_R2.
%
%  Discussion:
%
%    The rule has order
%
%      O = ( 4 * N^5 - 20 * N^4 + 140 * N^3 - 130 * N^2 + 96 * N + 15 ) / 15.
%
%    The rule has precision P = 11.
%
%    EN_R2 is the entire N-dimensional space with weight function
%
%      w(x) = exp ( - x1^2 - x2^2 ... - xn^2 )
%
%    There are two versions of each rule, chosen by setting the
%    OPTION variable to 1 or 2.
%
%    The rule as tabulated by Stenger is available for N = 2 through 20.
%    This function accepts N = 3 through 5.
%
%     N    O
%    __  ___
%     3  151
%     4  417
%     5  983
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%    3 <= N <= 5.
%
%    Input, integer OPTION, chooses rule option 1 or 2.
%
%    Output, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  if ( n < 3 | 5 < n )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EN_R2_11_1 - Fatal error!\n' );
    fprintf ( 1, '  3 <= N <= 5 required.\n' );
    error ( 'EN_R2_11_1 - Fatal error!' )
  end

  if ( nargin < 2 )
    option = 1;
  end

  if ( option < 1 | 2 < option )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EN_R2_11_1 - Fatal error!\n' );
    fprintf ( 1, '  1 <= OPTION <= 2 required.\n' );
    error ( 'EN_R2_11_1 - Fatal error!' )
  end

  o = ( 4 * n^5 - 20 * n^4 + 140 * n^3 - 130 * n^2 + 96 * n + 15 ) / 15;
  volume = sqrt ( pi^n );

  if ( n == 3 & option == 1 )
    u =     0.235060497367449E+01;
    v =     0.436077411927617E+00;
    w2 =    0.133584907401370E+01;
    b0 =  - 0.881591029957858E+01;
    b1 =  - 0.751996143360650E-01;
    b2 =    0.621743189471515E+01;
    b3 =    0.241426451456494E+00;
    b4 =  - 0.120709739276065E-02;
    b5 =  - 0.427751221210138E+01;
    b6 =    0.550169924840163E-01;
    b7 =    0.237084999634707E-01;
    b8 =  - 0.169791992887741E-02;
    b9 =  - 0.252266276123350E-04;
    b10 =   0.326777873717691E+01;
    b11 =   0.968469949206802E-02;
    b12 =   0.789754514877422E-03;
    b13 =   0.000000000000000E+00;
    b14 =   0.000000000000000E+00;
    b15 =   0.000000000000000E+00;
  elseif ( n == 3 & option == 2 )
    u =     0.235060497367449E+01;
    v =     0.133584907401370E+01;
    w2 =    0.436077411927617E+00;
    b0 =  - 0.141214037032900E+02;
    b1 =  - 0.803730274707282E-01;
    b2 =    0.235546545595906E+00;
    b3 =    0.888123191556611E+01;
    b4 =    0.142467131155533E-03;
    b5 =    0.582993124006494E-01;
    b6 =  - 0.561099173155661E+01;
    b7 =  - 0.204028691521686E-02;
    b8 =    0.252880089932256E-01;
    b9 =  - 0.814378678627283E-04;
    b10 =   0.804353953375146E-02;
    b11 =   0.393451849690453E+01;
    b12 =   0.171183493169724E-03;
    b13 =   0.000000000000000E+00;
    b14 =   0.000000000000000E+00;
    b15 =   0.000000000000000E+00;
  elseif ( n == 4 & option == 1 )
    u =     0.235060497367449E+01;
    v =     0.436077411927617E+00;
    w2 =    0.133584907401370E+01;
    b0 =    0.241502736147339E+03;
    b1 =  - 0.196095938531478E+00;
    b2 =  - 0.128675737999280E+03;
    b3 =    0.307568784278696E+00;
    b4 =  - 0.480908422319460E-02;
    b5 =    0.698087019367085E+02;
    b6 =    0.631837143743771E-01;
    b7 =    0.392226151971179E-01;
    b8 =  - 0.300948471646799E-02;
    b9 =  - 0.650235306755170E-04;
    b10 = - 0.386951974646715E+02;
    b11 =   0.171656829095787E-01;
    b12 =   0.139980343116450E-02;
    b13 =   0.101552487093372E-04;
    b14 =   0.222435922356439E+02;
    b15 =   0.000000000000000E+00;
  elseif ( n == 4 & option == 2 )
    u =     0.235060497367449E+01;
    v =     0.133584907401370E+01;
    w2 =    0.436077411927617E+00;
    b0 =  - 0.151944464736584E+03;
    b1 =  - 0.223498438689039E+00;
    b2 =    0.243574919068010E+00;
    b3 =    0.634373877008693E+02;
    b4 =  - 0.782065187814018E-04;
    b5 =    0.911833754536616E-01;
    b6 =  - 0.238927288245914E+02;
    b7 =  - 0.422314408318853E-02;
    b8 =    0.448218289217760E-01;
    b9 =  - 0.138053374667391E-03;
    b10 =   0.607473265800655E-02;
    b11 =   0.697375246129742E+01;
    b12 =   0.303414841680135E-03;
    b13 = - 0.314574391771792E-05;
    b14 =   0.409103498175100E-02;
    b15 =   0.000000000000000E+00;
  elseif ( n == 5 & option == 1 )
    u =     0.235060497367449E+01;
    v =     0.436077411927617E+00;
    w2 =    0.133584907401370E+01;
    b0 =    0.255885269311763E+04;
    b1 =  - 0.439598677491526E+00;
    b2 =  - 0.106541406144610E+04;
    b3 =    0.453540909054264E+00;
    b4 =  - 0.132100905623778E-01;
    b5 =    0.418606568954203E+03;
    b6 =    0.511394563043680E-01;
    b7 =    0.645581013845604E-01;
    b8 =  - 0.533417277494500E-02;
    b9 =  - 0.137981626254496E-03;
    b10 = - 0.147436933189884E+03;
    b11 =   0.304253807765057E-01;
    b12 =   0.248108698207828E-02;
    b13 =   0.113652094546015E-04;
    b14 =   0.394257407160391E+02;
    b15 =   0.331725011358320E-05;
  elseif ( n == 5 & option == 2 )
    u =     0.235060497367449E+01;
    v =     0.133584907401370E+01;
    w2 =    0.436077411927617E+00;
    b0 =  - 0.761305347548192E+03;
    b1 =  - 0.536360805019297E+00;
    b2 =    0.110669832078736E+00;
    b3 =    0.246421088923968E+03;
    b4 =  - 0.773649327968607E-03;
    b5 =    0.169088641205970E+00;
    b6 =  - 0.670700680243651E+02;
    b7 =  - 0.856090560229205E-02;
    b8 =    0.794446232770302E-01;
    b9 =  - 0.220272863263544E-03;
    b10 = - 0.373515812228225E-02;
    b11 =   0.123606544052884E+02;
    b12 =   0.537788804557843E-03;
    b13 = - 0.122101861480881E-04;
    b14 =   0.725117070759373E-02;
    b15 =   0.331725011358320E-05;
  end

  x = zeros(n,o);
  w = zeros(o,1);

  k = 0;
%
%  1 point.
%
  k = k + 1;
% x(1:n,k) = 0.0;
  w(k) = b0;
%
%  2 * N points.
%
  for i = 1 : n
    k = k + 1;
    x(i,k) = - u;
    w(k) = b1;
    k = k + 1;
    x(i,k) = + u;
    w(k) = b1;
  end
%
%  2 * N points.
%
  for i = 1 : n
    k = k + 1;
    x(i,k) = - v;
    w(k) = b2;
    k = k + 1;
    x(i,k) = + v;
    w(k) = b2;
  end
%
%  2 * N points.
%
  for i = 1 : n
    k = k + 1;
    x(i,k) = - w2;
    w(k) = b3;
    k = k + 1;
    x(i,k) = + w2;
    w(k) = b3;
  end
%
%  4 * ( N * ( N - 1 ) / 2 ) points.
%
  for i = 1 : n - 1
    for j = i + 1 : n
      k = k + 1;
      x(i,k) = - u;
      x(j,k) = - u;
      w(k) = b4;
      k = k + 1;
      x(i,k) = - u;
      x(j,k) = + u;
      w(k) = b4;
      k = k + 1;
      x(i,k) = + u;
      x(j,k) = - u;
      w(k) = b4;
      k = k + 1;
      x(i,k) = + u;
      x(j,k) = + u;
      w(k) = b4;
    end
  end
%
%  4 * ( N * ( N - 1 ) / 2 ) points.
%
  for i = 1 : n - 1
    for j = i + 1 : n
      k = k + 1;
      x(i,k) = - v;
      x(j,k) = - v;
      w(k) = b5;
      k = k + 1;
      x(i,k) = - v;
      x(j,k) = + v;
      w(k) = b5;
      k = k + 1;
      x(i,k) = + v;
      x(j,k) = - v;
      w(k) = b5;
      k = k + 1;
      x(i,k) = + v;
      x(j,k) = + v;
      w(k) = b5;
    end
  end
%
%  4 * ( N * ( N - 1 ) / 2 ) points.
%
  for i = 1 : n - 1
    for j = i + 1 : n
      k = k + 1;
      x(i,k) = - w2;
      x(j,k) = - w2;
      w(k) = b6;
      k = k + 1;
      x(i,k) = - w2;
      x(j,k) = + w2;
      w(k) = b6;
      k = k + 1;
      x(i,k) = + w2;
      x(j,k) = - w2;
      w(k) = b6;
      k = k + 1;
      x(i,k) = + w2;
      x(j,k) = + w2;
      w(k) = b6;
    end
  end
%
%  4 * ( N * ( N - 1 ) ) points.
%
  for i = 1 : n - 1
    for j = i + 1 : n
      k = k + 1;
      x(i,k) = - u;
      x(j,k) = - v;
      w(k) = b7;
      k = k + 1;
      x(i,k) = - u;
      x(j,k) = + v;
      w(k) = b7;
      k = k + 1;
      x(i,k) = + u;
      x(j,k) = - v;
      w(k) = b7;
      k = k + 1;
      x(i,k) = + u;
      x(j,k) = + v;
      w(k) = b7;
      k = k + 1;
      x(i,k) = - v;
      x(j,k) = - u;
      w(k) = b7;
      k = k + 1;
      x(i,k) = - v;
      x(j,k) = + u;
      w(k) = b7;
      k = k + 1;
      x(i,k) = + v;
      x(j,k) = - u;
      w(k) = b7;
      k = k + 1;
      x(i,k) = + v;
      x(j,k) = + u;
      w(k) = b7;
    end
  end
%
%  4 * ( N * ( N - 1 ) ) points.
%
  for i = 1 : n - 1
    for j = i + 1 : n
      k = k + 1;
      x(i,k) = - u;
      x(j,k) = - w2;
      w(k) = b8;
      k = k + 1;
      x(i,k) = - u;
      x(j,k) = + w2;
      w(k) = b8;
      k = k + 1;
      x(i,k) = + u;
      x(j,k) = - w2;
      w(k) = b8;
      k = k + 1;
      x(i,k) = + u;
      x(j,k) = + w2;
      w(k) = b8;
      k = k + 1;
      x(i,k) = - w2;
      x(j,k) = - u;
      w(k) = b8;
      k = k + 1;
      x(i,k) = - w2;
      x(j,k) = + u;
      w(k) = b8;
      k = k + 1;
      x(i,k) = + w2;
      x(j,k) = - u;
      w(k) = b8;
      k = k + 1;
      x(i,k) = + w2;
      x(j,k) = + u;
      w(k) = b8;
    end
  end
%
%  8 * ( N * ( N - 1 ) * ( N - 2 ) / 6 ) points.
%
  for i = 1 : n - 2
    for j = i + 1 : n - 1
      for l = j + 1 : n
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = - u;
        x(l,k) = - u;
        w(k) = b9;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = - u;
        x(l,k) = + u;
        w(k) = b9;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = + u;
        x(l,k) = - u;
        w(k) = b9;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = + u;
        x(l,k) = + u;
        w(k) = b9;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = - u;
        x(l,k) = - u;
        w(k) = b9;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = - u;
        x(l,k) = + u;
        w(k) = b9;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = + u;
        x(l,k) = - u;
        w(k) = b9;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = + u;
        x(l,k) = + u;
        w(k) = b9;
      end
    end
  end
%
%  8 * ( N * ( N - 1 ) * ( N - 2 ) / 6 ) points.
%
  for i = 1 : n - 2
    for j = i + 1 : n - 1
      for l = j + 1 : n
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = - v;
        x(l,k) = - v;
        w(k) = b10;
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = - v;
        x(l,k) = + v;
        w(k) = b10;
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = + v;
        x(l,k) = - v;
        w(k) = b10;
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = + v;
        x(l,k) = + v;
        w(k) = b10;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = - v;
        x(l,k) = - v;
        w(k) = b10;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = - v;
        x(l,k) = + v;
        w(k) = b10;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = + v;
        x(l,k) = - v;
        w(k) = b10;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = + v;
        x(l,k) = + v;
        w(k) = b10;
      end
    end
  end
%
%  8 * ( N * ( N - 1 ) * ( N - 2 ) / 6 ) points.
%
  for i = 1 : n - 2
    for j = i + 1 : n - 1
      for l = j + 1 : n
        k = k + 1;
        x(i,k) = - w2;
        x(j,k) = - w2;
        x(l,k) = - w2;
        w(k) = b11;
        k = k + 1;
        x(i,k) = - w2;
        x(j,k) = - w2;
        x(l,k) = + w2;
        w(k) = b11;
        k = k + 1;
        x(i,k) = - w2;
        x(j,k) = + w2;
        x(l,k) = - w2;
        w(k) = b11;
        k = k + 1;
        x(i,k) = - w2;
        x(j,k) = + w2;
        x(l,k) = + w2;
        w(k) = b11;
        k = k + 1;
        x(i,k) = + w2;
        x(j,k) = - w2;
        x(l,k) = - w2;
        w(k) = b11;
        k = k + 1;
        x(i,k) = + w2;
        x(j,k) = - w2;
        x(l,k) = + w2;
        w(k) = b11;
        k = k + 1;
        x(i,k) = + w2;
        x(j,k) = + w2;
        x(l,k) = - w2;
        w(k) = b11;
        k = k + 1;
        x(i,k) = + w2;
        x(j,k) = + w2;
        x(l,k) = + w2;
        w(k) = b11;
      end
    end
  end
%
%  8 * ( N * ( N - 1 ) * ( N - 2 ) / 2 ) points.
%
  for i = 1 : n - 2
    for j = i + 1 : n - 1
      for l = j + 1 : n
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = - u;
        x(l,k) = - v;
        w(k) = b12;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = - u;
        x(l,k) = + v;
        w(k) = b12;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = + u;
        x(l,k) = - v;
        w(k) = b12;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = + u;
        x(l,k) = + v;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = - u;
        x(l,k) = - v;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = - u;
        x(l,k) = + v;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = + u;
        x(l,k) = - v;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = + u;
        x(l,k) = + v;
        w(k) = b12;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = - v;
        x(l,k) = - u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = - v;
        x(l,k) = + u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = + v;
        x(l,k) = - u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = + v;
        x(l,k) = + u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = - v;
        x(l,k) = - u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = - v;
        x(l,k) = + u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = + v;
        x(l,k) = - u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = + v;
        x(l,k) = + u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = - u;
        x(l,k) = - u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = - u;
        x(l,k) = + u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = + u;
        x(l,k) = - u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = + u;
        x(l,k) = + u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = - u;
        x(l,k) = - u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = - u;
        x(l,k) = + u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = + u;
        x(l,k) = - u;
        w(k) = b12;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = + u;
        x(l,k) = + u;
        w(k) = b12;
      end
    end
  end
%
%  16 * ( N * ( N - 1 ) * ( N - 2 ) * ( N - 3 ) / 24 ) points.
%
  for i = 1 : n - 3
    for j = i + 1 : n - 2
      for l = j + 1 : n - 1
        for m = l + 1 : n
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = - u;
          x(l,k) = - u;
          x(m,k) = - u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = - u;
          x(l,k) = - u;
          x(m,k) = + u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = - u;
          x(l,k) = + u;
          x(m,k) = - u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = - u;
          x(l,k) = + u;
          x(m,k) = + u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = + u;
          x(l,k) = - u;
          x(m,k) = - u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = + u;
          x(l,k) = - u;
          x(m,k) = + u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = + u;
          x(l,k) = + u;
          x(m,k) = - u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = + u;
          x(l,k) = + u;
          x(m,k) = + u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = - u;
          x(l,k) = - u;
          x(m,k) = - u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = - u;
          x(l,k) = - u;
          x(m,k) = + u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = - u;
          x(l,k) = + u;
          x(m,k) = - u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = - u;
          x(l,k) = + u;
          x(m,k) = + u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = + u;
          x(l,k) = - u;
          x(m,k) = - u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = + u;
          x(l,k) = - u;
          x(m,k) = + u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = + u;
          x(l,k) = + u;
          x(m,k) = - u;
          w(k) = b13;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = + u;
          x(l,k) = + u;
          x(m,k) = + u;
          w(k) = b13;
        end
      end
    end
  end
%
%  16 * ( N * ( N - 1 ) * ( N - 2 ) * ( N - 3 ) / 24 ) points.
%
  for i = 1 : n - 3
    for j = i + 1 : n - 2
      for l = j + 1 : n - 1
        for m = l + 1 : n
          k = k + 1;
          x(i,k) = - v;
          x(j,k) = - v;
          x(l,k) = - v;
          x(m,k) = - v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = - v;
          x(j,k) = - v;
          x(l,k) = - v;
          x(m,k) = + v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = - v;
          x(j,k) = - v;
          x(l,k) = + v;
          x(m,k) = - v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = - v;
          x(j,k) = - v;
          x(l,k) = + v;
          x(m,k) = + v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = - v;
          x(j,k) = + v;
          x(l,k) = - v;
          x(m,k) = - v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = - v;
          x(j,k) = + v;
          x(l,k) = - v;
          x(m,k) = + v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = - v;
          x(j,k) = + v;
          x(l,k) = + v;
          x(m,k) = - v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = - v;
          x(j,k) = + v;
          x(l,k) = + v;
          x(m,k) = + v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = + v;
          x(j,k) = - v;
          x(l,k) = - v;
          x(m,k) = - v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = + v;
          x(j,k) = - v;
          x(l,k) = - v;
          x(m,k) = + v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = + v;
          x(j,k) = - v;
          x(l,k) = + v;
          x(m,k) = - v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = + v;
          x(j,k) = - v;
          x(l,k) = + v;
          x(m,k) = + v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = + v;
          x(j,k) = + v;
          x(l,k) = - v;
          x(m,k) = - v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = + v;
          x(j,k) = + v;
          x(l,k) = - v;
          x(m,k) = + v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = + v;
          x(j,k) = + v;
          x(l,k) = + v;
          x(m,k) = - v;
          w(k) = b14;
          k = k + 1;
          x(i,k) = + v;
          x(j,k) = + v;
          x(l,k) = + v;
          x(m,k) = + v;
          w(k) = b14;
        end
      end
    end
  end
%
%  All quintuples UUUUU with 32 sign combinations.
%
  for i1 = 1 : n - 4
    for i2 = i1 + 1 : n - 3
      for i3 = i2 + 1 : n - 2
        for i4 = i3 + 1 : n - 1
          for i5 = i4 + 1 : n
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = - u;
            x(i3,k) = - u;
            x(i4,k) = - u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = - u;
            x(i3,k) = - u;
            x(i4,k) = - u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = - u;
            x(i3,k) = - u;
            x(i4,k) = + u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = - u;
            x(i3,k) = - u;
            x(i4,k) = + u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = - u;
            x(i3,k) = + u;
            x(i4,k) = - u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = - u;
            x(i3,k) = + u;
            x(i4,k) = - u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = - u;
            x(i3,k) = + u;
            x(i4,k) = + u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = - u;
            x(i3,k) = + u;
            x(i4,k) = + u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = + u;
            x(i3,k) = - u;
            x(i4,k) = - u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = + u;
            x(i3,k) = - u;
            x(i4,k) = - u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = + u;
            x(i3,k) = - u;
            x(i4,k) = + u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = + u;
            x(i3,k) = - u;
            x(i4,k) = + u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = + u;
            x(i3,k) = + u;
            x(i4,k) = - u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = + u;
            x(i3,k) = + u;
            x(i4,k) = - u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = + u;
            x(i3,k) = + u;
            x(i4,k) = + u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = - u;
            x(i2,k) = + u;
            x(i3,k) = + u;
            x(i4,k) = + u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = - u;
            x(i3,k) = - u;
            x(i4,k) = - u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = - u;
            x(i3,k) = - u;
            x(i4,k) = - u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = - u;
            x(i3,k) = - u;
            x(i4,k) = + u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = - u;
            x(i3,k) = - u;
            x(i4,k) = + u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = - u;
            x(i3,k) = + u;
            x(i4,k) = - u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = - u;
            x(i3,k) = + u;
            x(i4,k) = - u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = - u;
            x(i3,k) = + u;
            x(i4,k) = + u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = - u;
            x(i3,k) = + u;
            x(i4,k) = + u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = + u;
            x(i3,k) = - u;
            x(i4,k) = - u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = + u;
            x(i3,k) = - u;
            x(i4,k) = - u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = + u;
            x(i3,k) = - u;
            x(i4,k) = + u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = + u;
            x(i3,k) = - u;
            x(i4,k) = + u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = + u;
            x(i3,k) = + u;
            x(i4,k) = - u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = + u;
            x(i3,k) = + u;
            x(i4,k) = - u;
            x(i5,k) = + u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = + u;
            x(i3,k) = + u;
            x(i4,k) = + u;
            x(i5,k) = - u;
            w(k) = b15;
            k = k + 1;
            x(i1,k) = + u;
            x(i2,k) = + u;
            x(i3,k) = + u;
            x(i4,k) = + u;
            x(i5,k) = + u;
            w(k) = b15;
          end
        end
      end
    end
  end

  return
end


function sF = cos(sF, varargin)
% cost of a function
% Syntax
%   sF = cos(sF)
%   sF = cos(sF, 'bandwidth', bandwidth)
%
% Input
%  sF - @S2FunHarmonic
%
% Output
%  sF - @S2FunHarmonic
%
% Options
%  bandwidth - minimal degree of the spherical harmonic
%

sF = sF.quadrature(@(v) cos(sF.eval(v)),varargin{:});

end


 function out = equivs(var1, var2, varargin)
%function out = equivs(var1, var2, command)
%|
%| verify that var1 and var2 are equivalent to within single precision accuracy
%| if not, print error message.  an alternative to isequal().
%| See also: jf_equal
%|
%| option
%|	'thresh'	threshold (default: 1e-6)
%|	'format'	format for displaying the two variables (default: '')
%|	'fail'	0|1	if 1 (default) then fail if not equivalent, else warn
%|
%| out
%|	out		0|1
%|
%| Copyright 2007, Jeff Fessler, University of Michigan

if nargin == 1 && streq(var1, 'test'), equivs_test, return, end
if nargin < 2, ir_usage, end

arg.thresh = 1e-6;
arg.format = '';
arg.fail = true;
arg = vararg_pair(arg, varargin);

if isempty(var1) && isempty(var2)
	ok = true;

elseif ~isequal(size(var1), size(var2))
	printm([': size(%s) = %s'], inputname(1), mat2str(size(var1)))
        printm([': size(%s) = %s'], inputname(2), mat2str(size(var2)))
	equivs_show(var1, var2, inputname(1), inputname(2), arg.format)
	if arg.fail
		fail('incompatible dimensions')
	else
		warn('incompatible dimensions')
		return
	end

else
	var1 = var1(:);
	var2 = var2(:);
	if any(isnan(var1)) || any(isnan(var2))
		if arg.fail
			fail('nan')
		else
			warn('nan')
			ok = false;
			return
		end
	end

	norm = (max(abs(var1)) + max(abs(var2))) / 2;
	if ~norm
		ok = true; % both zero!
	else
		err = max(abs(var1-var2)) / norm;
		ok = err < arg.thresh;
	end
end

if nargout
	out = ok;
end

if ok
	return
end

[name line] = caller_name;
if isempty(name)
	str = '';
else
	str = sprintf('%s %d:', name, line);
end

name1 = inputname(1);
name2 = inputname(2);
minmax(var1, ['equivs ' name1 ':'])
minmax(var2, ['equivs ' name2 ':'])
diff = var1 - var2;
if ~isreal(diff)
	diff = abs(diff);
end
minmax(diff)
printm([str ' normalized difference of %g between "%s" "%s"'], ...
	err, name1, name2);
if arg.fail
	fail('not equal to within single precision, thresh=%g', arg.thresh)
else
	warn('not equal to within single precision, thresh=%g', arg.thresh)
end

function equivs_show(var1, var2, name1, name2, format)
if isempty(format), return, end
disp(name1)
disp(num2str(var1, format))
disp(name2)
disp(num2str(var2, format))

function equivs_test
rng(0)
x = randn(1000,200);
y = dsingle(x);
equivs(x,y)

passed = 0;
try
	y = x + 2e-6 * max(x(:));
	equivs(x,y)
	passed = 1;
catch
end
if passed, error 'this should have failed!', end


load 'sdae_rbm_mnist_vis.mat';

colors = colormap;

figure;
hold on;
for c = 1:10
    x = H(X_labels == (c-1), 1);
    y = H(X_labels == (c-1), 2);
    rndidx = randperm(length(x));
    x = x(rndidx(1:500));
    y = y(rndidx(1:500));

    plot(x, y, 'x', 'Color', colors(ceil(c/10 * 64), :));
end
hold off;


%This function sorts the input array in ascending order using the Comb Sort algorithm
%For details, refer https://en.wikipedia.org/wiki/Comb_sort

function y = comb_sort(array)

len = length(array);
k = len;
isSwapped = true;
% value of shrink should be greater than 1
shrink = 1.4;
while ((k > 1) || (isSwapped == true))
    k = max(floor(k / shrink),1);
    % Bubble sort with given value of k
    i = 1;
    isSwapped = false;
    while ((i + k) <= len)
        if (array(i) > array(i + k))
            array = swap(array,i,i + k);
            isSwapped = true;
        end
        i = i + 1;
    end
end
y = array;
end

function array = swap(array,i,j)
value = array(i);
array(i) = array(j);
array(j) = value;
% Note: In practice, array should be passed by reference
end


%IM_THRESHOLD Fixed mapping thresholding images (DIP_Image)
%
%	B = IM_THRESHOLD(A,TYPE,PAR,INV)
%	B = A*IM_THRESHOLD([],TYPE,PAR,INV)
%	B = A*IM_THRESHOLD(TYPE,PAR,INV)
%
% INPUT
%   A        Dataset with object images (possibly multi-band)
%   TYPE     Type of procedure, see below
%   PAR      Related parameter
%   INV      If INV = 1, result inverted, default INV = 0.
%
% OUTPUT
%   B        Dataset with thresholded images
%
% DESCRIPTION
% The following procedures are supported (TYPE)
% 'isodata':      Thresholding using the Isodata algorithm
% 'otsu':         See IM2BW
% 'triangle':    *Thresholding using chord method
%                 (a.k.a. skewed bi-modality, maximum distance to triangle)
%                 by Zack, Rogers and Latt (1973).
% 'background':  *Thresholding using unimodal background-symmetry method.
% 'fixed':        Thresholding at a fixed value.
% 'double':      *Thresholding between two fixed values.
% 'volume':      *Thresholding to obtain a given volume fraction.
% 'hysteresis':  *From the binary image (in>low) only those regions are
%                 selected for which at least one pixel is (in>high)
%
% The following parameters are related to these procedures (PAR):
% ('background'): Distance to the peak where we cut-off, in
%                 terms of the half-width at half the maximum.
%                 Inf selects the default value, which is 2.
% ('fixed'):      Threshold value. Inf means halfway between
%                 minimum and maximum value.
% ('double'):     Two threshold values. Inf means min+[1/3,2/3]*(max-min).
% ('volume'):     Parameter = the volume fraction (Inf means 0.5)
% ('hysteresis'): Two values: low, high (see above)
%
% * routine needs DIP_IMAGE
%
% This routine is for smart thresholding. Simple thresholding can also be
% performed by B = A > PAR, which is the same as using the type 'fixed'.
%
% SEE ALSO (PRTools Guide)
% DATASETS, DATAFILES, DIP_IMAGE, THRESHOLD

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function b = im_threshold(varargin)

	argin = shiftargin(varargin,'char');
  argin = setdefaults(argin,[],'isodata',inf,0);
  if mapping_task(argin,'definition')
    b = define_mapping(argin,'fixed');
    b = setname(b,'Image threshold');
  else
    [a,type,par,inv] = deal(argin{:});
    if isa(a,'prdataset') % allows datafiles too
      isobjim(a);
      b = filtim(a,mfilename,{type,par,inv});
      b = setfeatsize(b,getfeatsize(a));
    elseif isa(a,'double') | isa(a,'dip_image') % here we have a single image
      switch lower(type)
        case('isodata')
          mina = min(a(:));  maxa = max(a(:));
          b = 255*(a-mina)/(maxa-mina);
          t0 = 128;
          for j=1:25
            U = b >  t0;
            L = b <= t0;
            n2 = mean(b(U)); n1 = mean(b(L));
            t1 = (n1+n2)/2;
            if t1 == t0;
              break;
            else
              t0 = t1;
            end
          end
          b = b > t0;
        case('fixed')
          b = a > par;
        case('otsu')
          b = im2bw(a);
        case {'triangle','background','double','volume','hysteresis'}
          checktoolbox('dipimage');
          a = 1.0*dip_image(a);
          b = threshold(a,type,par);
        otherwise
          error 'Illegal thresholding type'
      end
      if inv
        b = 1-b;
      end
    end
  end

return


function [ a_lu, pivot, info ] = r8gb_trf ( m, n, ml, mu, a )

%*****************************************************************************80
%
%% R8GB_TRF performs a LAPACK-style PLU factorization of a R8GB matrix.
%
%  Discussion:
%
%    The R8GB storage format is for an M by N banded matrix, with lower
%    bandwidth ML and upper bandwidth MU.  Storage includes room for ML
%    extra superdiagonals, which may be required to store nonzero entries
%    generated during Gaussian elimination.
%
%    The original M by N matrix is "collapsed" downward, so that diagonals
%    become rows of the storage array, while columns are preserved.  The
%    collapsed array is logically 2*ML+MU+1 by N.
%
%    This is a simplified, standalone version of the LAPACK
%    routine R8GBTRF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 March 2004
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Anderson, Bai, Bischof, Demmel, Dongarra, Du Croz, Greenbaum,
%    Hammarling, McKenney, Ostrouchov, Sorensen,
%    LAPACK User's Guide,
%    Second Edition,
%    SIAM, 1995.
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix A.  0 <= M.
%
%    Input, integer N, the number of columns of the matrix A.  0 <= N.
%
%    Input, integer ML, the number of subdiagonals within the band of A.
%    0 <= ML.
%
%    Input, integer MU, the number of superdiagonals within the band of A.
%    0 <= MU.
%
%    Input, real A(2*ML+MU+1,N), the matrix A in band storage.
%
%    Output, real A_LU(2*ML+MU+1,N), information about the PLU factorization.
%
%    Output, integer PIVOT(min(M,N)), the pivot indices;
%    for 1 <= i <= min(M,N), row i of the matrix was interchanged with
%    row IPIV(i).
%
%    Output, integer INFO, error flag.
%    = 0: successful exit;
%    < 0: an input argument was illegal;
%    > 0: if INFO = +i, U(i,i) is exactly zero. The factorization
%         has been completed, but the factor U is exactly
%         singular, and division by zero will occur if it is used
%         to solve a system of equations.
%
  info = 0;
  a_lu(1:2*ml+mu+1,1:n) = a(1:2*ml+mu+1,1:n);
%
%  KV is the number of superdiagonals in the factor U, allowing for fill-in.
%
  kv = mu + ml;
%
%  Set fill-in elements in columns MU+2 to KV to zero.
%
  for j = mu + 2 : min ( kv, n )
    for i = kv - j + 2 : ml
      a_lu(i,j) = 0.0E+00;
    end
  end
%
%  JU is the index of the last column affected by the current stage
%  of the factorization.
%
  ju = 1;

  for j = 1 : min ( m, n )
%
%  Set the fill-in elements in column J+KV to zero.
%
    if ( j + kv <= n )
      a_lu(1:ml,j+kv) = 0.0;
    end
%
%  Find the pivot and test for singularity.
%  KM is the number of subdiagonal elements in the current column.
%
    km = min ( ml, m-j );

    piv = abs ( a_lu(kv+1,j) );
    jp = kv + 1;

    for i = kv + 2 : kv + km + 1
      if ( piv < abs ( a_lu(i,j) ) )
        piv = abs ( a_lu(i,j) );
        jp = i;
      end
    end

    jp = jp - kv;

    pivot(j) = jp + j - 1;

    if ( a_lu(kv+jp,j) ~= 0.0E+00 )

      ju = max ( ju, min ( j+mu+jp-1, n ) );
%
%  Apply interchange to columns J to JU.
%
      if ( jp ~= 1 )
        for i = 0 : ju - j
          t = a_lu(kv+jp-i,j+i);
          a_lu(kv+jp-i,j+i) = a_lu(kv+1-i,j+i);
          a_lu(kv+1-i,j+i) = t;
        end
      end
%
%  Compute the multipliers.
%
      if ( 0 < km )

        a_lu(kv+2:kv+km+1,j) = a_lu(kv+2:kv+km+1,j) / a_lu(kv+1,j);
%
%  Update the trailing submatrix within the band.
%
        if ( j < ju )

          for k = 1 : ju-j

            if ( a_lu(kv+1-k,j+k) ~= 0.0E+00 )

              for i = 1 : km
                a_lu(kv+i+1-k,j+k) = a_lu(kv+i+1-k,j+k) ...
                  - a_lu(kv+i+1,j) * a_lu(kv+1-k,j+k);
              end
            end
          end
        end
      end

    else
%
%  If pivot is zero, set INFO to the index of the pivot
%  unless a zero pivot has already been found.
%
      if ( info == 0 )
        info = j
      end

    end

  end

  return
end


function S = globMatrixIFE3DPreCond(fun,coef,mesh,femI,fem1,fem2,dindx,dindy)

%% USAGE: generate stiffness global matrix on a 3D mesh
%
% INPUTS:
% fun --- coefficient function
% mesh --- a struct data contains very rich mesh information.
% fem1 --- global DoF for test function space
% fem2 --- global DoF for trial function space
%
% OUTPUTS:
% [IN JN XN] --- triplets of the sparse matrix from regular elements.
% [II JI XI] --- triplets of the sparse matrix from interface elements.

% Last Modified: 08/07/2020 by Xu Zhang

%% 0. Initializaiton
if strcmp(fem1.type,'P1')||strcmp(fem1.type,'DGP1')||strcmp(fem1.type,'CR')
    feEvalBas1 = @evalP1Bas3D;
elseif strcmp(fem1.type,'P2')||strcmp(fem1.type,'DGP2')
    feEvalBas1 = @evalP2Bas3D;
end

if strcmp(fem2.type,'P1')||strcmp(fem2.type,'DGP1')||strcmp(fem2.type,'CR')
    feEvalBas2 = @evalP1Bas3D;
elseif strcmp(fem2.type,'P2')||strcmp(fem2.type,'DGP2')
    feEvalBas2 = @evalP2Bas3D;
end

%% 1. Matrix on noninterface elements
dof1 = fem1.ldof; dof2 = fem2.ldof; nloc = dof1*dof2;
ntID = find(mesh.tLoc > 0); ntN = length(ntID);
AN = fem1.area(ntID);
gxN = fem1.gx(ntID,:); gyN = fem1.gy(ntID,:); gzN = fem1.gz(ntID,:); gw = fem1.gw;
XN = zeros(nloc*ntN, 1);

coefN = feval(fun,gxN,gyN,gzN);
Ibas = cell(dof1,1);
Jbas = cell(dof2,1);
for i = 1:dof1
    Ibas{i} = feEvalBas1(fem1.bas(ntID,:,i), gxN, gyN, gzN, dindx);
end
for j = 1:dof2
    Jbas{j} = feEvalBas2(fem2.bas(ntID,:,j), gxN, gyN, gzN, dindy);
end

IN = reshape(repmat(fem1.t(ntID,:),4,1),nloc*ntN,1);
JN = repmat(reshape(fem2.t(ntID,:),dof2*ntN,1),4,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        XN(ind+1:ind+ntN) = AN.*(sum(((Ibas{i}.*(coefN.*Jbas{j})).*gw'),2));
        ind = ind + ntN;
    end
end
ID = find(XN~=0);
SN = sparse(IN(ID),JN(ID),XN(ID),size(fem1.p,1),size(fem2.p,1));

%% 2. Matrix on interface elements
AI = femI.area; gw = femI.gw; gxI = femI.gx; gyI = femI.gy; gzI = femI.gz;
ntI = size(femI.t,1); % not number of interface element, but quadrature element
XI = zeros(nloc*ntI, 1);

coefI = (coef(1)+coef(2))/2;
Ibas = cell(dof1,1);
Jbas = cell(dof2,1);
for i = 1:dof1
    Ibas{i} = feEvalBas1(femI.bas(:,:,i), gxI, gyI, gzI, dindx);
end
for j = 1:dof2
    Jbas{j} = feEvalBas2(femI.bas(:,:,j), gxI, gyI, gzI, dindy);
end

II = reshape(repmat(femI.t,4,1),nloc*ntI,1);
JI = repmat(reshape(femI.t,dof2*ntI,1),4,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        XI(ind+1:ind+ntI) = AI.*(sum(((Ibas{i}.*(coefI.*Jbas{j})).*gw'),2));
        ind = ind + ntI;
    end
end
SI = sparse(II,JI,XI,size(fem1.p,1),size(fem2.p,1));
S = SN + SI;


function [gx,dG_dX,dG_dPhi,d2G_dXdPhi] = g_ERP(x,Phi,u,inG)
% neural mass observation function (DCM for ERPs)

n = size(x,1);          % should be 9
nPhi = length(Phi);     % should be 1

g = exp(Phi(1)).*[0  0   0   0   0   0   0   0   1
                  0  0   0   0   0   0   0   0   0.3
                  0  0   0   0   0   0   0   0   -0.2
                  0  0   0   0   0   0   0   0   -0.7];

% state observation
gx = g*x;

%------ gradients evaluations ------%

% wrt the hidden states
dG_dX = g';

% wrt the observation parameters
dG_dPhi = (g*x)';

% mixed partial derivatives
d2G_dXdPhi(:,:,1) = zeros(n,nPhi);
d2G_dXdPhi(9,1,1) = exp(Phi(1));
d2G_dXdPhi(:,:,2) = 0.3*d2G_dXdPhi(:,:,1);
d2G_dXdPhi(:,:,3) = -0.2*d2G_dXdPhi(:,:,1);
d2G_dXdPhi(:,:,4) = -0.7*d2G_dXdPhi(:,:,1);


function [fAphi] = calc_FaultStyle(rStress)
    % Determine the faulting style based on Quantifying Anderson's fault type
% [fAphi] = calc_FaultStyle(rStress)
% ------------------------------------------
% Determining the faulting style based on Quantifying Anderson's fault type
% by B. Simpson, 1997, JGR
%
% Incoming:
% rStress : Structure from script gui_NodeCalcStressInv giving stress
%           tensor inversion by Michael
% Output:
% fAphi   : 0 <= fAphi < 1 Normal faulting
%           1 <= fAphi < 2 Strike slip
%           2 <= fAphi < 3 Thrust faulting
%
% jowoe@gps.caltech.edu
% 19.05.2006

% Stress tensor from Michael inversion
mS=[rStress.fS11 rStress.fS12 rStress.fS13; rStress.fS12 rStress.fS22 rStress.fS23; rStress.fS13 rStress.fS23 rStress.fS33];

% Eigenvector; sorted from max. compressive to minimum (S1, S2, S3)
% Maximum compressive is the most negative value
vEig = eig(mS);

% Check for "most" vertical axis
vPlunge = [rStress.fS1Plunge; rStress.fS2Plunge; rStress.fS3Plunge];

vSel = (vPlunge == max(vPlunge));
fSigvert = vEig(vSel);

% This is because maximum compressive is negative
fSighmax = min(vEig(~vSel,:));
fSighmin = max(vEig(~vSel,:));

if fSighmax > fSigvert
    n = 0;
elseif fSigvert > fSighmin
    n = 2;
else
    n = 1;
end
% Formula 2, Simpson, JGR, 1997
fAphi = (n+0.5)+(-1)^n*(rStress.fPhi-0.5);


function y = glm_fit_model(x,k,v);
%
% y = glm_fit_model(x,k,v);
%
% A model used to generate a 'synthetic'
% autocorrelation function which may estimate
% temporally-correlated noise for within-
% skull voxels: see Burock and Dale, HBM 2000
% for more info.
%
% This is just broken off as a separate function
% for use in calling matlab's fminsearch routine.
%
% original code by gb, 11/04
% updated by ras, 05/05
y = norm((1 - x(1))*(x(2).^(1:k)') - v)^2;

return


function CPerm=permuteMatrix(C,order)
%%PERMUTEMATRIX Given an n1Xn2X...nS matrix, rearrange the dimensions of C
%      to be in the order specified by order. All elements of order must be
%      unique, real, positive, integer values from 1 to S. This functions
%      in the same manner as Matlab's permute function. The permutation
%      performed by this function is not done in-place. This type of
%      permutation is a type of tensor matrix transpose.
%
%INPUTS: C An n1Xn2X...XnS matrix.
%    order The length S order vector. This should hold integer values from
%          1 to S with no repeats.
%
%OUTPUTS: CPerm C with its dimensions permuted according to order.
%
%The algorithm is essentially brute force. It goes through each element in
%C and find the tuple of the associated element in CPerm to which the
%assignment must be performed.
%
%EXAMPLE:
% C=100*rand(3,4,5);
% order=[3,2,1];
% CPerm=permuteMatrix(C,order);
%One will ntoe that size(CPerm) is note [5,4,3].
%
%March 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

S=ndims(C);
numOrderEls=length(order);

if(isempty(order)&&~isempty(C))
   error('order cannot be empty with a non-empty C.')
end

if(numOrderEls<S)
    error('The ordering vector must be >=ndims(C).')
end

if(any(order>numOrderEls)||any(order<1))
    error('')
end

if(isempty(C))
    CPerm=[];
    return;
end

nVals=size(C);

if(numOrderEls>S)
   nVals=[nVals,ones(1,numOrderEls-S)];
   S=numOrderEls;
end

totalEls=numel(C);

cumProdNew=zeros(S,1);
nValsNew=zeros(S,1);
invPerm=zeros(S,1);
newIdx=zeros(S,1);%Initialization
idx=zeros(S,1);%Initialization

iNew=order(1);
nValsNew(1)=nVals(iNew);

invPerm(iNew)=1;
cumProdNew(1)=1;

for i=2:S
    iNew=order(i);

    nValsNew(i)=nVals(iNew);
    invPerm(iNew)=i;

    cumProdNew(i)=cumProdNew(i-1)*nValsNew(i-1);
end

CPerm=zeros(nValsNew(:)');

newLinIdx=0;
linIdx=0;
while(1)
    %Assign the current tuple.
    CPerm(newLinIdx+1)=C(linIdx+1);

    %Move on to the next tuple.
    linIdx=linIdx+1;

    if(linIdx>=totalEls)
        break;
    end

    %The code below is similar to that in getNextTuple, because we are
    %updating the tuples for newIdx.
    curLevel=1;
    while(1)
        %Try incrementing the order at this level.
        idx(curLevel)=idx(curLevel)+1;
        if(idx(curLevel)<nVals(curLevel))
            iNew=invPerm(curLevel);
            newIdx(iNew)=newIdx(iNew)+1;
            newLinIdx=newLinIdx+cumProdNew(iNew);

            idx(1:(curLevel-1))=0;
            break;
        else
            %If the value is invalid, then just keep ascending and
            %adjust newLinIdx.
            iNew=invPerm(curLevel);

            newLinIdx=newLinIdx-newIdx(iNew)*cumProdNew(iNew);
            newIdx(iNew)=0;

            curLevel=curLevel+1;
            continue;
        end
    end
end
end

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


function tests = test_stackSlice(varargin)
%TEST_STACKSLICE  One-line description here, please.
%
%   output = test_stackSlice(input)
%
%   Example
%   test_stackSlice
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-12-02,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

tests = functiontests(localfunctions);

function test_sliceX_gray(testCase) %#ok<*DEFNU>

img = createTestImage;
dim = stackSize(img);

sliceYZ = stackSlice(img, 1, 5);
assertEqual(testCase, [dim(3) dim(2)], size(sliceYZ));

sliceYZ = stackSlice(img, 'x', 5);
assertEqual(testCase, [dim(3) dim(2)], size(sliceYZ));

function test_sliceY_gray(testCase) %#ok<*DEFNU>

img = createTestImage;
dim = stackSize(img);

sliceZX = stackSlice(img, 2, 5);
assertEqual(testCase, [dim(1) dim(3)], size(sliceZX));

sliceZX = stackSlice(img, 'y', 5);
assertEqual(testCase, [dim(1) dim(3)], size(sliceZX));

function test_sliceZ_gray(testCase) %#ok<*DEFNU>

img = createTestImage;
dim = stackSize(img);

sliceXY = stackSlice(img, 3, 5);
assertEqual(testCase, [dim(2) dim(1)], size(sliceXY));

sliceXY = stackSlice(img, 'z', 5);
assertEqual(testCase, [dim(2) dim(1)], size(sliceXY));

function img = createTestImage

[x, y, z] = meshgrid(1:10, 1:15, 1:20);
img = 5*x + 4*y + 3*z;


function [output] = F_sigmoid(input_layer)
input = input_layer.a;
output = sigmoid(input);

end


function [ a, seed ] = r8vec_uniform ( n, b, c, seed )

%*****************************************************************************80
%
%% R8VEC_UNIFORM returns a scaled pseudorandom R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, real B, C, the range of the pseudorandom values.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(N), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_UNIFORM - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8VEC_UNIFORM - Fatal error!' );
  end

  for i = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + 2147483647;
    end

    a(i) = b + ( c - b ) * seed * 4.656612875E-10;

  end

  return
end


function test_failed=test_firwin
%TEST_FIRWIN  Test the firwin windows
%
%  This test script verifies the properties listed in the help of firwin

allwins = getfield(arg_firwin,'flags','wintype');

test_failed=0;

disp(' ===============  TEST_FIRWIN ================');

for L=[18,19,20,21]

  for cent=0:1
    if cent==0
      centtype='wp';
    else
      centtype='hp';
    end;
    for ii=1:length(allwins);
      winname=allwins{ii};

      [g,info]=firwin(winname,L,centtype);

      res = 1-isevenfunction(fir2long(g,2*L),centtype);
      [test_failed,fail]=ltfatdiditfail(res,test_failed);

      s=sprintf(['SYMM %10s %s L: %i %0.5g %s'],winname,centtype,L,res,fail);
      disp(s);

      if cent==0
        res=1-g(1);

        [test_failed,fail]=ltfatdiditfail(res,test_failed);

        s=sprintf(['PEAK %10s %s L: %i %0.5g %s'],winname,centtype,L,res,fail);
        disp(s);

      end;

      if mod(L,2)==0
        if info.ispu
          gpu=g+fftshift(g);
          res=norm(gpu-gpu(1)*ones(L,1));

          [test_failed,fail]=ltfatdiditfail(res,test_failed);

          s=sprintf(['PU   %10s %s L: %i %0.5g %s'],winname,centtype,L,res,fail);
          disp(s);

        end;

        if info.issqpu
          gpu=g.^2+fftshift(g.^2);
          res=norm(gpu-gpu(1)*ones(L,1));

          [test_failed,fail]=ltfatdiditfail(res,test_failed);

          s=sprintf(['SQPU %10s %s L: %i %0.5g %s'],winname,centtype,L,res,fail);
          disp(s);

        end;
      end;
    end;

  end;

end;


function peakmask = getpeaks(w)
   %GETPEAKS return mask for peak values for a waveform
   %     PEAKMASK = GETPEAKS(WAVEFORM)
   %     returns a mask of all maximums in the waveform.
   %
   %   Input Arguments
   %       WAVEFORM: waveform object  (SINGLE WAVEFORM)
   %
   %   Output
   %       PEAKMASK: a mask of all local maximums within the waveform.
   %       This mask can then be used to reference the peak points in a
   %       waveform
   %
   %   a MASK is a logical array the same size as the waveform's data,
   %   where the value is TRUE when that data point is a peak, and is zero
   %   otherwise.
   %
   %   in some cases, it may be preferable to get the peakmask for the
   %   absolute value of the data, that way both highs and lows are marked.
   %
   % See also WAVEFORM/HILBERT

   % AUTHOR: Celso Reyes, Geophysical Institute, Univ. of Alaska Fairbanks
   % $Date$
   % $Revision$

   if ~isscalar(w)
      error('Waveform:getpeaks:tooManyWaveforms',...
         'getpeaks can only be used with individual waveforms');
   end
   BiggerLeft = [(w.data(1:end-1) >= w.data(2:end)); true];
   BiggerRight = [true ; (w.data(1:end-1) < w.data(2:end))];
   peakmask = (BiggerLeft & BiggerRight);
end


% Example for the algorithms described in
%
%       D. Kressner, M. Steinlechner, A. Uschmajew:
%		Low-rank tensor methods with subspace correction for symmetric eigenvalue problems
%		SIAM J. Sci. Comput., 36(5):A2346-A2368, 2014.
%
% Code to produce Figure 4.4: Three Eigenvalues for Henon-Heiles with
% with n=28, d=10

%   TTeMPS Toolbox.
%   Michael Steinlechner, 2013-2016
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt
% =========================================================================
clear all
close all

n = 28;
d = 10;
A = TTeMPS_op_NN_hermite(n, d);
p = 3;
r = 1;

% Run block eigenvalue procedure:
% =========================================================================

if ~exist('hh_3_blk_hermite.mat','file')
    opts = struct( 'maxiter', 3, ...
                   'maxrank', 40, ...
                   'tol', 1e-8, ...
                   'tolOP', 1e-3, ...
                   'tolLOBPCG', 1e-6, ...
                   'maxiterLOBPCG', 500, ...
                   'verbose', true , ...
                   'precInner', true);

    rng(11)
    rr = [1, 1 * ones(1, d-1), 1];
    [X_blk_hermite, C_blk_hermite, evalue_blk_hermite, residuums_blk_hermite, micro_res_blk_hermite, objective_blk_hermite, t_blk_hermite] = block_eigenvalue( A, p, rr, opts);
    save('hh_3_blk_hermite', 'X_blk_hermite', 'C_blk_hermite', 'evalue_blk_hermite', 'residuums_blk_hermite', 'micro_res_blk_hermite', 'objective_blk_hermite','t_blk_hermite');
else
    load('hh_3_blk_hermite.mat')
end

% Run EVAMEn:
% =========================================================================

if ~exist('hh_3_evamen_hermite.mat','file')
    opts = struct( 'maxiter', 3, ...
                   'maxrank', 40, ...
                   'maxrankRes', 2, ...
                   'tol', 1e-8, ...
                   'tolOP', 1e-3, ...
                   'tolLOBPCG', 1e-6, ...
                   'maxiterLOBPCG', 500, ...
                   'verbose', true , ...
                   'precInner', true);
    rng(11)
    rr = [1, 1 * ones(1, d-1), 1];
    [X_evamen_hermite, C_evamen_hermite, evalue_evamen_hermite, residuums_evamen_hermite, micro_res_evamen_hermite, objective_evamen_hermite, t_evamen_hermite] = amen_eigenvalue( A, 1, p, rr, opts);
    save('hh_3_evamen_hermite', 'X_evamen_hermite', 'C_evamen_hermite', 'evalue_evamen_hermite', 'residuums_evamen_hermite', 'micro_res_evamen_hermite', 'objective_evamen_hermite','t_evamen_hermite');
else
    load('hh_3_evamen_hermite.mat')
end

% Prepare data for plotting:
% =========================================================================

evalue_end = repmat(evalue_blk_hermite(:,end), [1,size(evalue_blk_hermite,2)-1]);
ev_blk_hermite = abs(evalue_blk_hermite(:,1:end-1) - evalue_end);
ev_evamen_hermite = abs(evalue_evamen_hermite(:,1:end-1) - evalue_end);

% Plot vs. Iterations
% =========================================================================
f = figure
set(0,'defaultlinelinewidth',2)
subplot(1,2,1)

semilogy( sqrt(sum(micro_res_blk_hermite.^2, 1)), '-b' )
hold on
semilogy( sqrt(sum(micro_res_evamen_hermite.^2, 1)), '-k' )

semilogy( sum(ev_blk_hermite,1), '--b' )
semilogy( sum(ev_evamen_hermite,1), '--k' )

res_blk_hermite         = sqrt(sum(micro_res_blk_hermite.^2, 1))
res_evamen_hermite  = sqrt(sum(micro_res_evamen_hermite.^2, 1))

semilogy((d-1):(d-1):length(micro_res_blk_hermite),res_blk_hermite(:,(d-1):(d-1):end),'ob')
semilogy((d-1):(d-1):length(micro_res_evamen_hermite),res_evamen_hermite(:,(d-1):(d-1):end),'ok')

semilogy((d-1):(d-1):length(ev_blk_hermite),sum(ev_blk_hermite(:,(d-1):(d-1):end),1),'ob')
semilogy((d-1):(d-1):length(ev_evamen_hermite),sum(ev_evamen_hermite(:,(d-1):(d-1):end),1),'ok')

set(gca,'fontsize',20)
xlabel('Microiterations')
ylabel('Residual and eigenvalue error')

% Plot vs. Time
% =========================================================================

subplot(1,2,2)
semilogy( t_blk_hermite, sqrt(sum(micro_res_blk_hermite.^2, 1)), '-b' )
hold on
semilogy( t_evamen_hermite, sqrt(sum(micro_res_evamen_hermite.^2, 1)), '-k' )

semilogy( t_blk_hermite, sum(ev_blk_hermite,1), '--b' )
semilogy( t_evamen_hermite, sum(ev_evamen_hermite,1), '--k' )

semilogy(t_blk_hermite((d-1):(d-1):end),res_blk_hermite(:,(d-1):(d-1):end),'ob')
semilogy(t_evamen_hermite((d-1):(d-1):end),res_evamen_hermite(:,(d-1):(d-1):end),'ok')

semilogy(t_blk_hermite((d-1):(d-1):end),sum(ev_blk_hermite(:,(d-1):(d-1):end),1),'ob')
semilogy(t_evamen_hermite((d-1):(d-1):end),sum(ev_evamen_hermite(:,(d-1):(d-1):end),1),'ok')

semilogy(t_blk_hermite((d-1):(d-1):end),        res_blk_hermite(:,(d-1):(d-1):end),'ob')
semilogy(t_evamen_hermite((d-1):(d-1):end), res_evamen_hermite(:,(d-1):(d-1):end),'ok')

semilogy(t_blk_hermite((d-1):(d-1):end),        sum(ev_blk_hermite(:,(d-1):(d-1):end),1),'ob')
semilogy(t_evamen_hermite((d-1):(d-1):end), sum(ev_evamen_hermite(:,(d-1):(d-1):end),1),'ok')

h_leg = legend('Res. err., Block-ALS',...
       'Res. err. EVAMEn, prec.',...
       'EV. err., Block-ALS',...
       'EV. err. EVAMEn, prec.')
set(gca,'fontsize',20)
set(h_leg, 'fontsize',16)
xlabel('Time [s]')
ylabel('Residual and eigenvalue error')

set(f, 'Position', [0 0 1200 700])


function [ n_data, nu, x, fx ] = bessel_in_values ( n_data )

%*****************************************************************************80
%
%% BESSEL_IN_VALUES returns some values of the In Bessel function.
%
%  Discussion:
%
%    The modified Bessel functions In(Z) and Kn(Z) are solutions of
%    the differential equation
%
%      Z^2 W'' + Z * W' - ( Z^2 + N^2 ) * W = 0.
%
%    In Mathematica, the function can be evaluated by:
%
%      BesselI[n,x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer NU, the order of the function.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 28;

  fx_vec = [ ...
     0.5016687513894678E-02, ...
     0.1357476697670383E+00, ...
     0.6889484476987382E+00, ...
     0.1276466147819164E+01, ...
     0.2245212440929951E+01, ...
     0.1750561496662424E+02, ...
     0.2281518967726004E+04, ...
     0.3931278522104076E+08, ...
     0.2216842492433190E-01, ...
     0.2127399592398527E+00, ...
     0.1033115016915114E+02, ...
     0.1758380716610853E+04, ...
     0.2677764138883941E+21, ...
     0.2714631559569719E-03, ...
     0.9825679323131702E-02, ...
     0.2157974547322546E+01, ...
     0.7771882864032600E+03, ...
     0.2278548307911282E+21, ...
     0.2752948039836874E-09, ...
     0.3016963879350684E-06, ...
     0.4580044419176051E-02, ...
     0.2189170616372337E+02, ...
     0.1071597159477637E+21, ...
     0.3966835985819020E-24, ...
     0.4310560576109548E-18, ...
     0.5024239357971806E-10, ...
     0.1250799735644948E-03, ...
     0.5442008402752998E+19 ];

  nu_vec = [ ...
     2,  2,  2,  2, ...
     2,  2,  2,  2, ...
     3,  3,  3,  3, ...
     3,  5,  5,  5, ...
     5,  5, 10, 10, ...
    10, 10, 10, 20, ...
    20, 20, 20, 20 ];

  x_vec = [ ...
      0.2E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      2.5E+00, ...
      3.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     20.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    nu = 0;
    x = 0.0;
    fx = 0.0;
  else
    nu = nu_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function value = r4_besi1e ( x )

%*****************************************************************************80
%
%% R4_BESI1E: exponentially scaled Bessel function I of order 1 of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the exponentially scaled Bessel function I
%    of order 1 of X.
%
  persistent ai1cs
  persistent ai12cs
  persistent bi1cs
  persistent ntai1
  persistent ntai12
  persistent nti1
  persistent xmin
  persistent xsml

  if ( isempty ( nti1 ) )

    ai1cs = [ ...
     -0.02846744181881479E+00, ...
     -0.01922953231443221E+00, ...
     -0.00061151858579437E+00, ...
     -0.00002069971253350E+00, ...
      0.00000858561914581E+00, ...
      0.00000104949824671E+00, ...
     -0.00000029183389184E+00, ...
     -0.00000001559378146E+00, ...
      0.00000001318012367E+00, ...
     -0.00000000144842341E+00, ...
     -0.00000000029085122E+00, ...
      0.00000000012663889E+00, ...
     -0.00000000001664947E+00, ...
     -0.00000000000166665E+00, ...
      0.00000000000124260E+00, ...
     -0.00000000000027315E+00, ...
      0.00000000000002023E+00, ...
      0.00000000000000730E+00, ...
     -0.00000000000000333E+00, ...
      0.00000000000000071E+00, ...
     -0.00000000000000006E+00 ]';
    ai12cs = [ ...
      0.02857623501828014E+00, ...
     -0.00976109749136147E+00, ...
     -0.00011058893876263E+00, ...
     -0.00000388256480887E+00, ...
     -0.00000025122362377E+00, ...
     -0.00000002631468847E+00, ...
     -0.00000000383538039E+00, ...
     -0.00000000055897433E+00, ...
     -0.00000000001897495E+00, ...
      0.00000000003252602E+00, ...
      0.00000000001412580E+00, ...
      0.00000000000203564E+00, ...
     -0.00000000000071985E+00, ...
     -0.00000000000040836E+00, ...
     -0.00000000000002101E+00, ...
      0.00000000000004273E+00, ...
      0.00000000000001041E+00, ...
     -0.00000000000000382E+00, ...
     -0.00000000000000186E+00, ...
      0.00000000000000033E+00, ...
      0.00000000000000028E+00, ...
     -0.00000000000000003E+00 ]';
    bi1cs = [ ...
     -0.001971713261099859E+00, ...
      0.40734887667546481E+00, ...
      0.034838994299959456E+00, ...
      0.001545394556300123E+00, ...
      0.000041888521098377E+00, ...
      0.000000764902676483E+00, ...
      0.000000010042493924E+00, ...
      0.000000000099322077E+00, ...
      0.000000000000766380E+00, ...
      0.000000000000004741E+00, ...
      0.000000000000000024E+00 ]';

    nti1 = r4_inits ( bi1cs, 11, 0.1 * r4_mach ( 3 ) );
    ntai1 = r4_inits ( ai1cs, 21, 0.1 * r4_mach ( 3 ) );
    ntai12 = r4_inits ( ai12cs, 22, 0.1 * r4_mach ( 3 ) );
    xmin = 2.0 * r4_mach ( 1 );
    xsml = sqrt ( 8.0 * r4_mach ( 3 ) );

  end

  y = abs ( x );

  if ( x == 0.0 )
    value = 0.0;
  elseif ( y <= xmin )
    value = 0.0;
  elseif ( y <= xsml )
    value = 0.5 * x;
    value = exp ( - y ) * value;
  elseif ( y <= 3.0 )
    value = x * ( 0.875 ...
      + r4_csevl ( y * y / 4.5 - 1.0, bi1cs, nti1 ) );
    value = exp ( - y ) * value;
  elseif ( y <= 8.0 )
    value = ( 0.375 + r4_csevl ( ( 48.0 / y - 11.0 ) / 5.0, ...
      ai1cs, ntai1) ) / sqrt ( y );
    if ( x < 0.0 )
      value = - value;
    end
  else
    value = ( 0.375 + r4_csevl ( 16.0 / y - 1.0, ai12cs, ntai12 ) ) ...
      / sqrt ( y );
    if ( x < 0.0 )
      value = - value;
    end
  end

  return
end


function rotError = get_rot_error(R_est,R_gt,d)

nrNodes = size(R_est,1) / d;
rotError = zeros(nrNodes, 1);
for i=1:nrNodes
    Rdiff = R_gt(blkIndices(i,d),:)' * R_est(blkIndices(i,d),:);
    rotError(i, 1) = abs( atan2(Rdiff(2, 1), Rdiff(1, 1)) );
end


% NaN    Not-a-Number.
%    NaN is the IEEE arithmetic representation for Not-a-Number.
%    A NaN is obtained as a result of mathematically undefined
%    operations like 0.0/0.0  and inf-inf.
%
%    NaN('double') is the same as NaN with no inputs.
%
%    NaN('single') is the single precision representation of NaN.
%
%    NaN(N) is an N-by-N matrix of NaNs.
%
%    NaN(M,N) or NaN([M,N]) is an M-by-N matrix of NaNs.
%
%    NaN(M,N,P,...) or NaN([M,N,P,...]) is an M-by-N-by-P-by-... array of NaNs.
%
%    NaN(..., CLASSNAME) is an array of NaNs of class specified by the
%    string CLASSNAME. CLASSNAME can be either 'single' or 'double'.
%
%    NaN(..., 'like', Y) is an array of NaNs with the same data type, sparsity,
%    and complexity (real or complex) as the single or double precision numeric
%    variable Y.
%
%    Note: The size inputs M, N, and P... should be nonnegative integers.
%    Negative integers are treated as 0.
%
%    See also INF, ISNAN, ISFINITE, ISFLOAT.
%
%    Reference page in Doc Center
%       doc nan
%
%    Other functions named nan
%
%       codistributed/nan      codistributor2dbc/nan    gpuArray/nan
%       codistributor1d/nan    distributed/nan          ts/nan
%


function [tm,im] = maprect(tr,pr)
%MAPRECT find the tree-to-rectangle mappings.
%   [TM,IM] = MAPRECT(TR,PR) returns the tree-to-rectangle
%   and rectangle-to-tree mappings for a given aabb-tree TR
%   and a collection of query vertices PI.
%
%   The tree-to-item mapping TM is a structure representing
%   the intersection of the items PI with the tree TR. TM.II
%   is an M-by-1 array of tree indices and TM.LL is an
%   M-by-1 cell array of item lists. Specifically, items in
%   the list TM.LL{JJ} intersect with the node TM.II(JJ).
%
%   The item-to-tree mapping IM is a structure representing
%   the inverse mapping. IM.II is an N-by-1 array of item
%   indices and IM.LL is an N-by-1 cell array of node lists.
%   Specifically, nodes in the list IM.LL{JJ} intersect with
%   the item IM.II(JJ).
%
%   See also QUERYSET, MAPVERT, MAKETREE

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 09/04/2017

%----------------------- call SCANTREE to do the actual work
    if (nargout == +1)
       [tm   ] = scantree(tr,pr,@partrect);
    else
       [tm,im] = scantree(tr,pr,@partrect);
    end

end

function [j1,j2] = partrect(pr,b1,b2)
%PARTRECT partition points between boxes B1,B2 for SCANTREE.

    j1 = true(size(pr,1),1) ;
    j2 = true(size(pr,1),1) ;

    nd = size(b1,2) / +2;

    for ax = +1 : nd
%--------------- remains TRUE if inside bounds along axis AX
    j1 = j1 & pr(:,ax+nd*1) ...
           >= b1(  ax+nd*0) ...
            & pr(:,ax+nd*0) ...
           <= b1(  ax+nd*1) ;

%--------------- remains TRUE if inside bounds along axis AX
    j2 = j2 & pr(:,ax+nd*1) ...
           >= b2(  ax+nd*0) ...
            & pr(:,ax+nd*0) ...
           <= b2(  ax+nd*1) ;
    end

end


function [mm2] = cm22mm2(cm2)
% Convert area from square centimeters to square millimeters.
% Chad A. Greene 2012
mm2 = cm2*100;


function X = normalize(X,N,normtype,mode)
%NORMALIZE Normalizes the columns of the factor matrices.
%
%   NORMALIZE(X) normalizes the columns of each factor matrix using the
%   vector 2-norm, absorbing the excess weight into lambda. Also ensures
%   that lambda is positive.
%
%   NORMALIZE(X,N) absorbs the weights into the Nth factor matrix instead
%   of lambda. (All the lambda values are 1.)
%
%   NORMALIZE(X,0) equally divides the weights across the factor matrices.
%   (All the lambda values are 1.)
%
%   NORMALIZE(X,[]) is equivalent to NORMALIZE(X).
%
%   NORMALIZE(X,'sort') is the same as the above except it sorts the
%   components by lambda value, from greatest to least.
%
%   NORMALIZE(X,V,1) normalizes using the vector one norm (sum(abs(x))
%   rather than the two norm (sqrt(sum(x.^2))), where V can be any of the
%   second arguments decribed above.
%
%   NORMALIZE(X,[],1,I) just normalizes the I-th factor using whatever norm
%   is specified by the 3rd argument (1 or 2).
%
%   See also KTENSOR, ARRANGE, REDISTRIBUTE, TOCELL.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%%
if ~exist('N','var')
    N = -1;
end

if isempty(N)
    N = -1;
end

if isequal(N,'sort')
    N = -2;
end

if ~exist('normtype','var')
    normtype = 2;
end

if exist('mode', 'var')
    for r = 1:length(X.lambda)
        tmp = norm(X.u{mode}(:,r),normtype);
        if (tmp > 0)
            X.u{mode}(:,r) = X.u{mode}(:,r) / tmp;
        end
        X.lambda(r) = X.lambda(r) * tmp;
    end
    return;
end

%% Ensure that matrices are normalized
for r = 1:length(X.lambda)
    for n = 1:ndims(X)
        tmp = norm(X.u{n}(:,r),normtype);
        if (tmp > 0)
            X.u{n}(:,r) = X.u{n}(:,r) / tmp;
        end
        X.lambda(r) = X.lambda(r) * tmp;
    end
end

%% Check that all the lambda values are positive
idx = find(X.lambda < 0);
X.u{1}(:,idx) = -1 * X.u{1}(:,idx);
X.lambda(idx) = -1 * X.lambda(idx);

%% Absorb the weight into one factor, if requested
if (N == 0)
    D = diag(nthroot(X.lambda,ndims(X)));
    X.u = cellfun(@(x) x*D, X.u, 'UniformOutput', false);
    X.lambda = ones(size(X.lambda));
elseif (N > 0)
    X.u{N} = X.u{N} * diag(X.lambda);
    X.lambda = ones(size(X.lambda));
elseif (N == -2)
    if ncomponents(X) > 1
        [~,p] = sort(X.lambda,'descend');
        X = arrange(X,p);
    end
end


function combo_test36 ( )

%*****************************************************************************80
%
%% COMBO_TEST36 tests SETPART_ENUM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, ' \n' );
  fprintf ( 1, 'COMBO_TEST36\n' );
  fprintf ( 1, '  Set partitions:\n' );
  fprintf ( 1, ' \n' );
  fprintf ( 1, '  SETPART_ENUM enumerates,\n' );
  fprintf ( 1, ' \n' );
%
%  Enumerate.
%
  for n = 1 : 6
    npart = setpart_enum ( n );
    fprintf ( 1, '  %4d  %4d\n', n, npart );
  end

  return
end


function writeDoublePendulumDynamics(f,M)

%This function writes the dynamics file for the double pendulum.

filename = 'doublePendulumDynamics';

comments{1} = ['DZ = ' upper(filename) '(T,Z,U,P)'];
comments{2} = ' ';
comments{3} = 'FUNCTION:  This function computes the dynamics of a double';
comments{4} = '    pendulum, and is designed to be called from ode45. The';
comments{5} = '    model allows for arbitrary mass and inertia for each';
comments{6} = '    link, but no friction or actuation';
comments{7} = ' ';
comments{8}  = 'INPUTS: ';
comments{9}  = '    t = time. Dummy input for ode45. Not used.';
comments{10} = '    z = [4xn] matrix of states.';
comments{11} = '    u = [2xn] matrix of inputs';
comments{12} = '    P = struct of parameters';
comments{13} = 'OUTPUTS: ';
comments{14} = '    dz = [4xn] matrix of state derivatives';
comments{15} = ' ';
comments{16} = 'NOTES:';
comments{17} = ['    This file was automatically generated by ' mfilename];

params{1} = {'m1','link one mass'};
params{2} = {'m2','link two mass'};
params{3} = {'g ','gravity'};
params{4} = {'l1','link one length'};
params{5} = {'l2','link two length'};
params{6} = {'I1','link one moment of inertia about its center of mass'};
params{7} = {'I2','link two moment of inertia about its center of mass'};
params{8} = {'d1','distance between link one center of mass and parent joint'};
params{9} = {'d2','distance between link two center of mass and parent joint'};

states{1} = {'th1','link one absolute angle'};
states{2} = {'dth1','link one angular rate'};
states{3} = {'th2','link two absolute angle'};
states{4} = {'dth2','link two angular rate'};

input{1} = {'u1','torque acting on link 1 wrt ground'};
input{2} = {'u2','torque acting on link 2 wrt ground'};

dstates{1} = {'dth1','derivative of link one absolute angle'};
dstates{2} = {'ddth1','derivative of link one angular rate'};
dstates{3} = {'dth2','derivative of link two absolute angle'};
dstates{4} = {'ddth2','derivative of link two angular rate'};

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                               write file                                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
fid = fopen([filename '.m'],'w');

fprintf(fid, ['function dz = ' filename '(~,z,u,P) \n']);

for i=1:length(comments)
    fprintf(fid,['%%' comments{i} '\n']);
end
fprintf(fid,'\n');

for i=1:length(params)
    fprintf(fid,[params{i}{1} ' = P.' params{i}{1} '; %%' params{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:length(states)
    fprintf(fid,[states{i}{1} ' = z(' num2str(i) ',:); %%' states{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:length(input)
    fprintf(fid,[input{i}{1} ' = u(' num2str(i) ',:); %%' input{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:2;
    fprintf(fid,['f' num2str(i) ' = ' vectorize(char(f(i))) ';\n']);
end
fprintf(fid,'\n');

for i=1:2
    for j=1:2
        fprintf(fid,['M' num2str(i) num2str(j) ' = ' vectorize(char(M(i,j))) ';\n']);
    end
end
fprintf(fid,'\n');

fprintf(fid,'D = M11.*M22 - M12.*M21;\n\n');

fprintf(fid,'ddth1 = (f2.*M12 - f1.*M22)./D;\n');
fprintf(fid,'ddth2 = -(f2.*M11 - f1.*M21)./D;\n');
fprintf(fid,'\n');

fprintf(fid,'dz = [...\n');
for i=1:length(dstates)
    fprintf(fid,['    ' dstates{i}{1} '; %%' dstates{i}{2} '\n']);
end
fprintf(fid,'];\n');

fprintf(fid,'end \n');

fclose(fid);
end


%compute orientation of tailcentralvector

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

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

for i=1:numlarvae
    larva=larvae(i);
    tailcentralang{1,i}=bsxfun(@atan2,trx(larva).ycentral_mm-trx(larva).ytail_mm,trx(larva).xcentral_mm-trx(larva).xtail_mm);
end
units=parseunits('rad');
data=tailcentralang;


% Jiao Xianjun (putaoshu@msn.com; putaoshu@gmail.com)
% generate frequency domain PSS and time domain PSS signal
% A script of project: https://github.com/JiaoXianjun/rtl-sdr-LTE

function [fd_pss, td_pss] = pss_gen
fd_pss = zeros(128,3);
td_pss = zeros(128+9,3);
for i=1:3
    temp=pss(i-1);
    fd_pss(:,i)=[0 temp(32:end) zeros(1,65) temp(1:31)].';
    temp_td=idft(fd_pss(:,i))*sqrt(128/62);
    td_pss(:,i)=[temp_td(end-8:end); temp_td];
end


function s = addnaka(s)
%ADDNAKA Add the Nakagami distribution.

%   Copyright 1993-2004 The MathWorks, Inc.
%   $Revision: 1.1.6.7 $  $Date: 2003/12/11 03:50:49 $

j = length(s) + 1;
s(j).name = 'Nakagami';
s(j).code = 'nakagami';
s(j).pnames = {'mu' 'omega'};
s(j).pdescription = {'shape' 'scale'};
s(j).prequired = [false false];
s(j).fitfunc = @nakafit;
s(j).likefunc = @nakalike;
s(j).cdffunc = @nakacdf;
s(j).pdffunc = @nakapdf;
s(j).invfunc = @nakainv;
s(j).statfunc = @nakastat;
s(j).loginvfunc = [];
s(j).logcdffunc = [];
s(j).hasconfbounds = false;
s(j).censoring = true;
s(j).paramvec = true;
s(j).support = [0 Inf];
s(j).closedbound = [false false];
s(j).iscontinuous = true;
s(j).islocscale = false;
s(j).uselogpp = false;

% ==== Nakagami distribution functions ====

% these distribution functions do not yet handle arrays of parameters

function y = nakapdf(x, mu, omega)
%NAKAPDF Nakagami probability density function (pdf).
mu(mu <= 0) = NaN;
omega(omega <= 0) = NaN;

x(x<0) = 0;
% equivalent to y = 2.*x .* gampdf(x.^2, mu, omega./mu), but the version here
% puts all the x terms into gampdf, so Inf*0, etc. is handled there.
y = 2.*sqrt(omega./mu).*exp(gammaln(mu+.5) - gammaln(mu)) .* gampdf(x.^2, mu+.5, omega./mu);

function p = nakacdf(x, mu, omega)
%NAKACDF Nakagami cumulative distribution function (cdf).
mu(mu <= 0) = NaN;
omega(omega <= 0) = NaN;

x(x<0) = 0;
p = gamcdf(x.^2, mu, omega./mu);

function x = nakainv(p, mu, omega)
%NAKAINV Inverse of the Nakagami cumulative distribution function (cdf).
mu(mu <= 0) = NaN;
omega(omega <= 0) = NaN;

x = sqrt(gaminv(p,mu,omega./mu));

function r = nakarnd(mu, omega, varargin)
%NAKARND Random arrays from the Nakagami distribution.
mu(mu <= 0) = NaN;
omega(omega <= 0) = NaN;

[err, sizeOut] = statsizechk(2,mu,omega,varargin{:});
if err > 0
    error('stats:nakarnd:InconsistentSizes','Size information is inconsistent.');
end

r = sqrt(gamrnd(mu,omega./mu,sizeOut));

function [m,v] = nakastat(mu, omega)
%NAKASTAT Mean and variance for the Nakagami distribution.
mu(mu <= 0) = NaN;
omega(omega <= 0) = NaN;

gamratio = exp(gammaln(mu+.5) - gammaln(mu));
m = gamratio .* sqrt(omega./mu);
v = omega .* (1 - gamratio.^2 ./ mu);

function [nlogL,acov] = nakalike(params,data,cens,freq)
%NAKALIKE Negative log-likelihood for the Nakagami distribution.
if nargin < 4 || isempty(freq), freq = ones(size(data)); end
if nargin < 3 || isempty(cens), cens = zeros(size(data)); end

nlogL = naka_nloglf(params, data, cens, freq);
if nargout > 1
    acov = mlecov(params, data, 'nloglf',@naka_nloglf, 'cens',cens, 'freq',freq);
end

% ==== Nakagami fitting functions ====

function [phat,pci] = nakafit(x,alpha,cens,freq,opts)
%NAKAFIT Parameter estimates and confidence intervals for Nakagami data.

if nargin < 2 || isempty(alpha), alpha = .05; end
if nargin < 3 || isempty(cens), cens = zeros(size(x)); end
if nargin < 4 || isempty(freq), freq = ones(size(x)); end
if nargin < 5, opts = []; end

if any(x <= 0)
    error('stats:nakafit:BadData','The data in X must be positive');
end

phat = gamfit(x.^2,alpha,cens,freq,opts);
phat(2) = phat(1).*phat(2); % (a,b) -> (mu,omega)
if nargout > 1
    acov = mlecov(phat, x, 'nloglf',@naka_nloglf, 'cens',cens, 'freq',freq);
    probs = [alpha/2; 1-alpha/2];
    se = sqrt(diag(acov))';
    pci = norminv(repmat(probs,1,numel(phat)), [phat; phat], [se; se]);
    % CI on the log scale for omega?
end

function [nll,ngrad] = naka_nloglf(parms, x, cens, freq)
%NAKA_NLOGLF Objective function for Nakagami maximum likelihood.

% do all the calculations in terms of the gamma dist'n
a = parms(1);
b = parms(2)./parms(1); % (mu,omega) -> (a,b)
loggama = gammaln(a);
logb = log(b);

xsq = x.^2;
z = xsq ./ b;
logz = log(z);
L = (a-1).*logz - z - loggama - logb + log(2.*x);
ncen = sum(freq.*cens);
if ncen > 0
    cen = (cens == 1);
    zcen = z(cen);
    if nargout == 1
        Scen = gammainc(zcen,a,'upper');
    else
        [dScen,Scen] = dgammainc(zcen,a,'upper');
    end
    L(cen) = log(Scen);
end
nll = -sum(freq .* L);

if nargout > 1
    dL1 = logz - psi(a);
    dL2 = (z - a)./b;
    if ncen > 0
        dL1(cen) = dScen ./ Scen;
        dL2(cen) = exp(a.*logz(cen) - logb - zcen - loggama) ./ Scen;
    end
    ngrad = -[sum(freq .* dL1) sum(freq .* dL2)];

    % transform back to Nakagami parameters
    ngrad = ngrad * [1 0; -b./a 1./a]; % (a,b) -> (mu,omega)
end


function g = grad(SO3F,varargin)
% right-sided gradient of an SO3Fun
%
% Syntax
%   G = SO3F.grad % compute the gradient
%   g = SO3F.grad(rot) % evaluate the gradient in rot
%
%   % go 5 degree in direction of the gradient
%   ori_new = exp(rot,5*degree*normalize(g))
%
% Input
%  SO3F - @SO3FunCBF
%  rot  - @rotation / @orientation
%
% Output
%  G - @SO3VectorField
%  g - @vector3d
%
% Description
% general formula:
%
% $$s(g1_i) = sum_j c_j DRK(<g h_j,r_j>) g h_j x r_j $$
%

% fallback to generic method
if check_option(varargin,'check') || nargin == 1 || ~isa(varargin{1},'rotation')
  g = grad@SO3Fun(SO3F,varargin{:});
  return
end

ori = varargin{1};

% symmetrise - only crystal symmetry
[h,l] = symmetrise(SO3F.h.normalize,'unique');
r = repelem(SO3F.r.normalize,l);
w = repelem(SO3F.weights./l,l);

g = vector3d.zeros(size(ori));
for i = 1:length(h)
  g = g + w(i) * SO3F.psi.grad(dot(ori*h(i),r(i),'noSymmetry'),'polynomial') .* ...
      cross(h(i),inv(ori) * r(i));
end
end

function test

cs = crystalSymmetry('1');
odf = fibreODF(Miller(0,0,1,cs),vector3d.Z);
omega = linspace(-20,20)*degree;
omega = 15 *degree;
ref = orientation.byAxisAngle(vector3d(1,0,10),omega,cs)

g1 = odf.grad(ref)
g2 = odf.grad(ref,'check','delta',0.05*degree)

plot(omega./degree,[g1.x,g2.x])

omega2 = linspace(-5,5)*degree;
ori1 = ref * rotation.byAxisAngle(g1,omega2);
ori2 = ref * rotation.byAxisAngle(g2,omega2);
ori3 = ref * rotation.byAxisAngle(normalize(g1+g2),omega2);
%ori4 = ref * rotation.byAxisAngle(normalize(g1-g2),'angle',omega2);
ori4 = ref * rotation.byAxisAngle(vector3d(-1,2,0),omega2);

plot(omega2./degree,[odf.eval(ori1(:)),odf.eval(ori2(:)),odf.eval(ori3(:)),odf.eval(ori4(:))])

end

function test2

  cs = crystalSymmetry('321');
  odf = fibreODF(Miller(1,2,3,cs),vector3d(-1,3,2));

  ref = orientation.rand(1000,cs)

  g1 = odf.grad(ref)
  g2 = odf.grad(ref,'check','delta',0.05*degree)

  hist(norm(g1-g2)./degree)

  ref = orientation.id(cs) * cs(5);

  f = S2FunHarmonic.quadrature(@(r) ...
    odf.eval(rotation.byAxisAngle(r,5*degree)*ref));

  plot(f,'lower')

  annotate(odf.grad(ref))
end


function [EEG, globalTrendOut] = removeGlobalTrend(EEG, globalTrendIn)
% Perform detrending or high pass filtering to remove low frequency
%
% Usage:
%   EEG = removeTrend(EEG)
%   [EEG, detrendOut] = removeTrend(EEG, detrendIn)
%
% Input:
%   EEG               Structure that requires .data and .srate fields
%   detrendIn         Input structure with fields described below
%
% Structure parameters (detrendIn):
%   detrendChannels   Vector of channels to detrend or filter
%                     (default is all channels)
%   detrendType       One of the strings 'high pass', 'linear', or 'none'
%                     indicating type of detrending (default is'linear')
%   detrendCutoff     Detrend or high pass cutoff (default is 1 Hz)
%   detrendStepSize   Seconds for detrend window slide (default is 0.02 s)
%
% Output:
%   EEG               Revised EEG structure channels detrended or filtered
%   detrendOut        Structure with the following items described below:
%
% Structure parameters (detrendOut):
%   detrendChannels   Vector of detrended or filtered channels
%   detrendType       Type of detrending or filtering
%   detrendCutoff     High pass cutoff or detrend window (default is 1 Hz)
%   detrendStepSize   Seconds for detrend window slide (default is 0.02 s)
%   detrendCommand    String version of detrending command
%
% Implementation notes:
%   1) High pass filtering is done with EEGLAB pop_eegfiltnew FIR filter
%   2) Detrending is done with the chronux_2 runline command
%   3) The EEG.data array will be converted to double regardless of type
%
%% Check the parameters
if nargin < 1 || ~isstruct(EEG)
    error('removeGlobalTrend:NotEnoughArguments', 'first argument must be a structure');
elseif nargin < 2 || ~exist('globalTrendIn', 'var') || isempty(globalTrendIn)
    globalTrendIn = struct();
end
if ~isstruct(globalTrendIn)
    error('removeGlobalTrend:NoData', 'second argument must be a structure')
end

defaults = getPrepDefaults(EEG, 'globaltrend');
globalTrendOut = struct('globalTrendChannels', [], 'doLocal', [], ...
    'localCutoff', [], 'localStepSize', [], 'linearFit', [], ...
     'channelCorrelations', []);
[globalTrendOut, errors] = checkPrepDefaults(globalTrendIn, globalTrendOut, defaults);
if ~isempty(errors)
    error('removeGlobalTrend:BadParameters', ['|' sprintf('%s|', errors{:})]);
end
%% Detrend the data either using high pass or linear detrending
if globalTrendOut.doLocal
    params = struct('detrendChannels', globalTrendOut.globalTrendChannels, ...
                    'detrendType', 'linear', ...
                    'detrendCutoff', globalTrendOut.localCutoff, ...
                    'detrendStepSize', globalTrendOut.localStepSize);

    [EEG, trend] = removeTrend(EEG, params);
    globalTrendOut.localTrend = trend;
end

channels = 1:size(EEG.data, 1);
globalTrendOut.globalTrendChannels = channels;
t = 0:(size(EEG.data, 2) - 1);
t = t/EEG.srate;
data = EEG.data(channels, :);
myPval = zeros(size(data, 1), 2);
myCorr = zeros(size(data, 1), 1);
parfor k = 1:size(data, 1)
  myPval(k, :) = polyfit(t, data(k, :), 1);
  myCorr(k) = corr(t', (data(k, :))');
  data(k, :) = data(k, :) - polyval(myPval(k, :), t);
end
globalTrendOut.linearFit = myPval;
globalTrendOut.channelCorrelations = myCorr;
EEG.data(channels, :) = data;


function P = putcolor11(A11, sizeP)
%------------------------------------------------------------------------------
%
% Gridfunction A11 is upsampled.
%
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: June 6, 1999.
% (c) 1999-2002 Stichting CWI, Amsterdam
%------------------------------------------------------------------------------
[n, m] = size(A11);
if nargin == 2
  nP = sizeP(1);
  mP = sizeP(2);
  if nP < 2*n
    error(' putcolor11 - 1st dimension of P too small ')
  end
  if mP < 2*m
    error(' putcolor11 - 2nd dimension of P too small ')
  end
elseif nargin == 1
  nP = 2*n+1;
  mP = 2*m+1;
else
  error(' putcolor11 - wrong number of arguments ')
end
P=reshape(linspace(0,0,nP*mP),nP,mP);
P(2:2:nP, 2:2:mP)=A11;
%------------------------------------------------------------------------------


function [annotation, img] = LMimpad(annotation, img, PADSIZE, PADVAL)
% [annotation, img] = LMimpad(annotation, img, PADSIZE, PADVAL)
%
% [annotation, img] = LMimpad(annotation, img, [256 256], 0)
% PADSIZE = nrows x ncols (final image size)
% PADVAL = value for the border

[nrows, ncols, cols]=size(img);

if PADSIZE(1)<nrows || PADSIZE(2)<ncols
    error('ERROR: image is larger than target size. Use LMimcrop instead.')
end

Dy = fix((PADSIZE(1)-nrows)/2);
Dx = fix((PADSIZE(2)-ncols)/2);

img = [repmat(PADVAL, [PADSIZE(1) Dx cols]) ...
    [repmat(PADVAL, [Dy ncols cols]); img; repmat(PADVAL, [PADSIZE(1)-nrows-Dy ncols cols])] ...
    repmat(PADVAL, [PADSIZE(1) PADSIZE(2)-ncols-Dx cols])];

% Change the polygon coordinates
if isfield(annotation, 'object')
    Nobjects = length(annotation.object); n=0;
    for i = 1:Nobjects
        [x,y] = getLMpolygon(annotation.object(i).polygon);
        x = round(x + Dx);
        y = round(y + Dy);
        annotation.object(i).polygon = setLMpolygon(x,y);

%         Npoints = length(annotation.object(i).polygon.pt);
%         for j = 1:Npoints
%             x=str2num(annotation.object(i).polygon.pt(j).x);
%             y=str2num(annotation.object(i).polygon.pt(j).y);
%
%             x = round(x + Dx);
%             y = round(y + Dy);
%
%             annotation.object(i).polygon.pt(j).x = num2str(x);
%             annotation.object(i).polygon.pt(j).y = num2str(y);
%         end
    end
end


function x = nonzeros(a)
%NONZEROS     Implements  nonzeros(a)  for sparse interval matrix
%
%   x = nonzeros(a)
%
%Functionality as in Matlab.
%

% written  08/09/02     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 08/25/07     S.M. Rump  huge indices for sparse matrices
% modified 02/18/08     S.M. Rump  no column vector output for ND arrays
% modified 10/18/08     S.M. Rump  huge arrays
% modified 05/31/09     S.M. Rump  multiple arrays
% modified 10/03/12     S.M. Rump  spones for ND-arrays
% modified 11/13/12     S.M. Rump  cure Matlab 7.0 bug
%

  if a.complex
    if isequal(a.rad,0)
      x = cintval(nonzeros(a.mid));
    else
      [m,n] = size(a.mid);          % n is product of remaining indices for multiple arrays
      if length(n)>1                % cure bug in Matlab 7.0 (R14)
        a.mid = reshape(a.mid,m,n);   % make sure dimension fits with sparse(...)
        a.rad = reshape(a.rad,m,n);   % a.rad cannot be 0
      end
      [I,J] = find(spones(a.mid)+spones(a.rad));    % correct index set
      xmid = complex( real(nonzeros(real(a.mid)+sparse(I,J,sqrt(-1),m,n))) , ...
                      real(nonzeros(imag(a.mid)+sparse(I,J,sqrt(-1),m,n))) );
      xrad = real( real(nonzeros(a.rad+sparse(I,J,sqrt(-1),m,n))) );
      x = intval(xmid,xrad,'midrad');
      if size(x.mid,1)==1
        x.mid = x.mid.';
        x.rad = x.rad';
      end
    end
  else
    [m,n] = size(a.inf);
    if prod(size(a.inf))<2^31       % not huge
      if issparse(a.inf)
        x = nonzeros(a.inf(:)+sqrt(-1)*a.sup(:));
      else
        x = nonzeros(complex(a.inf(:),a.sup(:)));
      end
      x = intval(real(x),imag(x),'infsup');
      return
      I = find(spones(a.inf)+spones(a.sup));
      x = intval(squeeze(a.inf(I)),squeeze(a.sup(I)),'infsup');
      sizexinf = size(x.inf);
      if ( length(sizexinf)<3) & ( size(x.inf,1)==1 )
        x.inf = x.inf';
        x.sup = x.sup';
      end
    else                            % huge array
      [I,J,asup] = find(a.sup);
      x = nonzeros(a.inf + sparse(I,J,complex(0,asup),m,n));
      x = intval(real(x),imag(x),'infsup');
    end
  end


function ci =  cusum(cat)
    % This function calculates the CUMSUm function (Page 1954).
    %

    report_this_filefun(mfilename('fullpath'));

    [existFlag,figNumber]=figure_exists('CUSUM',1); %find figure with name 'CUSUM', and do not pop to foreground

    if existFlag
        cfig = figNumber;
    else
        cfig=figure_w_normalized_uicontrolunits(...                  %build figure for plot
            'Units','normalized','NumberTitle','off',...
            'Name','CUSUM',...
            'MenuBar','none',...
            'visible','off',...
            'pos',[ 0.300  0.3 0.4 0.6]);
        ho=false;

        matdraw
    end   % if fig exist

    m  = cat(:,6);
    me = mean(m);
    i = (1:1:length(m));
    ci = cumsum(m)' - i.*me;

    figure_w_normalized_uicontrolunits(cfig)
    delete(gca);delete(gca);
    plot(cat(:,3),ci,'o')
    %plot(i,ci,'o')
    set(gca,'visible','on','FontSize',10,'FontWeight','normal',...
        'FontWeight','normal','LineWidth',1.0,...
        'Box','on')
    xlabel('Time [yrs]')
    ylabel('CUSUM [yrs]')
    grid