 % Copyright 2001, Brown University, Providence, Rhode Island.
 %
 % All Rights Reserved
 %
 % Permission to use this software for noncommercial research and
 % educational purposes is hereby granted without fee.
 % Redistribution, sale, or incorporation of this software into a
 % commercial product is prohibited.
 %
 % BROWN UNIVERSITY DISCLAIMS ANY AND ALL WARRANTIES WITH REGARD TO
 % THIS SOFTWARE,INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
 % AND FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL BROWN
 % UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
 % DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE,
 % DATA OR PROFITS.

% ----------------------------------------------------------------
%  jacobd() - derivative of jacobi polynomials - vector version
%
%  Get a vector 'poly' of values of the derivative of the n_th order
%  Jacobi polynomial P^(alpha,beta)_N(z) at the np points z.
%
%  To do this we have used the relation
%
%  d   alpha,beta   1                  alpha+1,beta+1
%  -- P (z)       = -(alpha+beta+n+1) P  (z)
%  dz  n            2                  n-1
%  ----------------------------------------------------------------*/

function jd = jacobd(z, n, alpha, beta)

 jd = zeros(size(z));

  one = 1.0;
  if(n == 0)
    jd(:,:) = 0.0;
  else
    jd = am282jacobi1d(z,n-1,alpha+one,beta+one);
    jd = jd*0.5*(alpha + beta + n + one);
  end


function [c,Ls]=dgt2(f,g1,p3,p4,p5,p6)
%DGT2  2-D Discrete Gabor transform
%   Usage: c=dgt2(f,g,a,M);
%          c=dgt2(f,g1,g2,[a1,a2],[M1,M2]);
%          c=dgt2(f,g1,g2,[a1,a2],[M1,M2],[L1,L2]);
%          [c,Ls]=dgt2(f,g1,g2,[a1,a2],[M1,M2]);
%          [c,Ls]=dgt2(f,g1,g2,[a1,a2],[M1,M2],[L1,L2]);
%
%   Input parameters:
%         f       : Input data, matrix.
%         g,g1,g2 : Window functions.
%         a,a1,a2 : Length of time shifts.
%         M,M1,M2 : Number of modulations.
%         L1,L2   : Length of transform to do
%
%   Output parameters:
%         c       : array of coefficients.
%         Ls      : Original size of input matrix.
%
%   `dgt2(f,g,a,M)` will calculate a separable two-dimensional discrete
%   Gabor transformation of the input signal *f* with respect to the window
%   *g* and parameters *a* and *M*.
%
%   For each dimension, the length of the transform will be the smallest
%   possible that is larger than the length of the signal along that dimension.
%   f will be appropriately zero-extended.
%
%   `dgt2(f,g,a,M,L)` computes a Gabor transform as above, but does
%   a transform of length *L* along each dimension. *f* will be cut or
%   zero-extended to length *L* before the transform is done.
%
%   `[c,Ls]=dgt2(f,g,a,M)` or `[c,Ls]=dgt2(f,g,a,M,L)` additionally returns
%   the length of the input signal *f*. This is handy for reconstruction::
%
%                [c,Ls]=dgt2(f,g,a,M);
%                fr=idgt2(c,gd,a,Ls);
%
%   will reconstruct the signal *f* no matter what the size of *f* is, provided
%   that *gd* is a dual window of *g*.
%
%   `dgt2(f,g1,g2,a,M)` makes it possible to use a different window along the
%   two dimensions.
%
%   The parameters *a*, *M*, *L* and *Ls* can also be vectors of length 2.
%   In this case the first element will be used for the first dimension
%   and the second element will be used for the second dimension.
%
%   The output *c* has *4* or *5* dimensions. The dimensions index the
%   following properties:
%
%   1. Number of translation along 1st dimension of input.
%
%   2. Number of channel along 1st dimension  of input
%
%   3. Number of translation along 2nd dimension of input.
%
%   4. Number of channel along 2nd dimension  of input
%
%   5. Plane number, corresponds to 3rd dimension of input.
%
%   See also:  dgt, idgt2, gabdual

complainif_argnonotinrange(nargin,4,6,mfilename);

L=[];

if prod(size(p3))>2
  % Two windows was specified.
  g2=p3;
  a=p4;
  M=p5;
  if nargin==6
    L=p6;
  end;
else
  g2=g1;
  a=p3;
  M=p4;
  if nargin==5
    L=p5;
  end;
end;

if isempty(L)
  L1=[];
  L2=[];
else
  L1=L(1);
  L2=L(2);
end;

% Expand 'a' and M if necessary to two elements
a=bsxfun(@times,a,[1 1]);
M=bsxfun(@times,M,[1 1]);

Ls=size(f);
Ls=Ls(1:2);

c=dgt(f,g1,a(1),M(1),L1);
c=dgt(c,g2,a(2),M(2),L2,'dim',3);


function discon_meas = localized_discontinuity_measure(topographies, channel_locations, n_ics)
% localized_discontinuity_measure calculates a measure of how discontinuous
% an IC scalp map is. A high value indicates that the IC might represent an
% artifact. A very similar measure was used for the ADJUST toolbox,
% http://www.unicog.org/pm/pmwiki.php/MEG/RemovingArtifactsWithADJUST
% Our weighting scheme for electrodes surrounding the electrode for which
% the measure is being calculated is a little different.
%
% Input:
% topographies: Matrix of topographies (scalp maps) of ICs. Each row
% corresponds to an IC, and each column to an electrode, so it is the
% transpose of the field icawinv in EEGLab data structures.
%
% channel_locations: Channel locations of the electrodes whose activations
% are given in topographies.
%
% n_ics: The number of ICs represented in topographies.
%
% Output:
% discon_meas: A vector of discontinuity measures for each IC. For each IC,
% the discontinuity measure is found by first calculating how peaked the
% spatial map is for each electrode. The discontinuity measure is then the
% highest value found.

% This code is based heavily on the code from the ADJUST toolbox. It was
% modified by Laura Froelich 24/4/2013.
%
% Copyright (C) 2009 Andrea Mognon and Marco Buiatti,
% Center for Mind/Brain Sciences, University of Trento, Italy
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

xpos=[channel_locations.X];ypos=[channel_locations.Y];zpos=[channel_locations.Z];
pos=[xpos',ypos',zpos'];
discon_meas=zeros(1,n_ics);

for ic=1:n_ics
    aux=zeros(1,length(channel_locations));
    for el=1:length(channel_locations)-1

        P=pos(el,:); %position of current electrode
        d=pos-repmat(P,length(channel_locations),1);
        dist=sqrt(sum((d.*d),2));

        [y,I]=sort(dist, 'ascend'); % sort electrodes in ascending order of distance, so closest are first in list
        repchas=I(2:end);
        weightdist=exp(-y(2:end)); % weights computed wrt distance
        weightdist=weightdist/sum(weightdist); % ensure that weights sum to one

        aux(el)=abs(topographies(ic,el)-sum(weightdist.*topographies(ic,repchas)')); % Since the sum
        % of weights is one, the quantity
        % sum(weightdist.*topographies(ic,repchas)'), is a weighted
        % average. Hence this calculation finds the difference between the
        % activation in the current electrode and the average activation
        % surrounding it.
    end

    discon_meas(ic)=max(aux);
end


function geometry_test043 ( )

%*****************************************************************************80
%
%% TEST043 tests SEGMENTS_DIST_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST043\n' );
  fprintf ( 1, '  SEGMENTS_DIST_3D computes the distance between\n' );
  fprintf ( 1, '    line segments in 3D.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Case   Computed    True\n' );
  fprintf ( 1, '\n' );
%
%  Case 1, parallel, not coincident.
%
%  LS1: (2,3,0) + t * (2,1,0) for t = 0 to 3.
%  LS2: (11,6,4) + t * (2,1,0) for t = 0 to 3.
%  Distance is 5.
%
  p1(1:3,1) = [  2.0; 3.0; 0.0 ];
  p2(1:3,1) = [  8.0; 6.0; 0.0 ];
  q1(1:3,1) = [ 11.0; 6.0; 4.0 ];
  q2(1:3,1) = [ 17.0; 9.0; 4.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  1  %8f  5.0\n', dist );
%
%  Case 2, parallel, coincident, overlapping.
%
%  (1,2,3) + t * ( 1,-1,2)
%  LS1: t = 0 to t = 3.
%  Distance is 0.
%
  p1(1:3,1) = [  1.0;  2.0;  3.0 ];
  p2(1:3,1) = [  4.0; -1.0;  9.0 ];
  q1(1:3,1) = [  3.0;  0.0;  7.0 ];
  q2(1:3,1) = [  6.0; -3.0; 13.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 2, '  1  %8f  0.0\n', dist );
%
%  Case 3, parallel, coincident, disjoint.
%
%  LS1: (3,4,5) + t * ( 2,2,1) for 0 <= t <= 2.
%  LS2: (3,4,5) + t * ( 2,2,1) for 3 <= t <= 5.
%  Distance = 3.
%
  p1(1:3,1) = [  3.0;  4.0;  5.0 ];
  p2(1:3,1) = [  7.0;  8.0;  7.0 ];
  q1(1:3,1) = [  9.0; 10.0;  8.0 ];
  q2(1:3,1) = [ 13.0; 14.0; 10.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  3  %8f  3.0\n', dist );
%
%  Case 4, nonparallel, could intersect, and does intersect.
%
%  L1: (1,1,1) + t * (0,1,2)
%  L2: (0,2,3) + t * (1,0,0)
%  intersect at (1,2,3)
%  Distance is 0.
%
  p1(1:3,1) = [  1.0;  1.0;  1.0 ];
  p2(1:3,1) = [  1.0;  4.0;  7.0 ];
  q1(1:3,1) = [  0.0;  2.0;  3.0 ];
  q2(1:3,1) = [  5.0;  2.0;  3.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  4  %8f  0.0\n', dist );
%
%  Case 5, nonparallel, could intersect, and does not intersect.
%
%  L1: (1,1,1) + t * (0,1,2)
%  L2: (0,2,3) + t * (1,0,0)
%  lines intersect at (1,2,3), line segments do not.
%  Distance is 1.0
%
  p1(1:3,1) = [  1.0;  1.0;  1.0 ];
  p2(1:3,1) = [  1.0;  4.0;  7.0 ];
  q1(1:3,1) = [  0.0;  2.0;  3.0 ];
  q2(1:3,1) = [ -5.0;  2.0;  3.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  5  %8f  1.0\n', dist );
%
%  Case 6, nonparallel, can not intersect, "end-to-end".
%
%  L1: (2,2,1) + t * (0,1,2)  0 <= t <= 5
%  L2: (0,0,0) + t * (-1,-1,-1) 0 <= t <= 5
%  Distance is 3.
%
  p1(1:3,1) = [  2.0;  2.0;  1.0 ];
  p2(1:3,1) = [  2.0;  7.0;  11.0 ];
  q1(1:3,1) = [  0.0;  0.0;  0.0 ];
  q2(1:3,1) = [ -5.0; -5.0; -5.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  6  %8f  3.0\n', dist );
%
%  Case 7, nonparallel, can not intersect, "end-to-mid".
%
%  L1: (1,1,1) + t * (0,1,2) 0 <= t <= 5
%  L2: (0,4,7) + t * (-1,0,0) 0 <= t <= 5
%  Distance is 1.
%
  p1(1:3,1) = [  1.0;  1.0;  1.0 ];
  p2(1:3,1) = [  1.0;  6.0; 11.0 ];
  q1(1:3,1) = [  0.0;  4.0;  7.0 ];
  q2(1:3,1) = [ -5.0;  4.0;  7.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  7  %8f  1.0\n', dist );
%
%  Case 8, nonparallel, can not intersect, "mid-to-mid".
%
%  L1: (0,5,10) + t * (1,-1,0) 0 <= t <= 5
%  L2: (0,0,0) + t * (1,1,0) 0 <= t <= 6
%  Distance = 10.
%
  p1(1:3,1) = [  0.0;  5.0; 10.0 ];
  p2(1:3,1) = [  5.0;  0.0; 10.0 ];
  q1(1:3,1) = [  0.0;  0.0;  0.0 ];
  q2(1:3,1) = [  6.0;  6.0;  0.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  8  %8f 10.0\n', dist );
%
%  Case 9, nonparallel, can not intersect, "mid-to-end".
%
%  L1: (-2,0,0) + t * (1,0,0) 0 <= t <= 12
%  L2: (-2,8,1) + t * (9,-4,-1) 0 <= t <= 1
%  Distance = 4.
%
  p1(1:3,1) = [ -2.0;  0.0;  0.0 ];
  p2(1:3,1) = [ 10.0;  0.0;  0.0 ];
  q1(1:3,1) = [ -2.0;  8.0;  1.0 ];
  q2(1:3,1) = [  7.0;  4.0;  0.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  9  %8f  4.0\n', dist );

  return
end


function value = r8_cosh ( x )

%*****************************************************************************80
%
%% R8_COSH evaluates the hyperbolic cosine of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 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 hyperbolic cosine of X.
%
  persistent ymax

  if ( isempty ( ymax ) )
    ymax = 1.0 / sqrt ( r8_mach ( 3 ) );
  end

  y = exp ( abs ( x ) );

  if ( y < ymax )
    value = 0.5 * ( y + 1.0 / y );
  else
    value = 0.5 * y;
  end

  return
end


function [error_x,error_y] = ...
         navierpost_bc(viscosity,aez,fezx,fezy,elerrorx,elerrory,xy,ev,ebound);
%navierpost_q1p0_bc   postprocesses Poisson error estimator
%   [error_x,error_y] = ...
%   navierpost_q1p0_bc(viscosity,aez,fezx,fezy,elerrorx,elerrory,xy,ev,ebound);;
%   input
%          viscosity            viscosity parameter
%          aez                  elementwise Poisson problem matrices
%          fezx,fezy            elementwise rhs vectors
%          elerrorx, elerrory   elementwise error estimate (without BC imposition)
%          xy                   vertex coordinate vector
%          ev                   element mapping matrix
%          ebound               element edge boundary matrix
%   output
%          error_x, error_y     component elementwise error estimate
%
%   calls function localbc_xycd
%   IFISS function: DJS; 11 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      x=xy(:,1); y=xy(:,2);
      nel=length(ev(:,1));
      lev=[ev,ev(:,1)];
	  error_x=elerrorx;  error_y=elerrory;
%
% recompute contributions from elements with Dirichlet boundaries
      nbde=length(ebound(:,1));
      ebdy = zeros(nel,1);
      edge = zeros(nel,1);
% isolate boundary elements
      for el = 1:nbde
      ee = ebound(el,1);
      ebdy(ee) = ebdy(ee)+1; edge(ee)=ebound(el,2);
      end
%
% two edge elements
      k2=find(ebdy==2);
      nel2b=length(k2);
% loop over two edge elements
      for el = 1:nel2b
      el2e=k2(el);
      kk=find(ebound(:,1) == el2e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
	  ae=squeeze(aez(el2e,1:5,1:5));
      fex=fezx(el2e,:)'; fey=fezy(el2e,:)';
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el2e,:)); yl=y(lev(el2e,:));
	  [bae,fex,fey] = localbc_xycd(viscosity,ae,fex,fey,edges,xl,yl);
% solve local problems
      errx=viscosity*bae\fex;  erry=viscosity*bae\fey;
	  error_x(el2e,1) = errx'*ae*errx; error_y(el2e,1) = erry'*ae*erry;
	  end
% end of element loop
%
% one edge elements
      k1=find(ebdy==1);
      nel1b=length(k1);
% loop over one edge elements
      for el = 1:nel1b
      el1e=k1(el);
      kk=find(ebound(:,1) == el1e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
      fex=fezx(el1e,:)'; fey=fezy(el1e,:)';
	  ae=squeeze(aez(el1e,1:5,1:5));
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el1e,:)); yl=y(lev(el1e,:));
	  [bae,fex,fey] = localbc_xycd(viscosity,ae,fex,fey,edges,xl,yl);
% solve local problems
      errx=viscosity*bae\fex;  erry=viscosity*bae\fey;
	  error_x(el1e,1) = errx'*ae*errx; error_y(el1e,1) = erry'*ae*erry;
      end
% end of element loop
%
      err_x = sqrt(sum(error_x)); error_x = sqrt(error_x);
	  err_y = sqrt(sum(error_y)); error_y = sqrt(error_y);
      fprintf('estimated velocity error (in energy):  (%10.6e,%10.6e) \n',err_x,err_y)
 return


function out = traj_cost_lgr(opt_vars,traj_par,baseQR)
% -------------------------------------------------------------------
% This function computes cost in terms of condition number for
% trajectory optimization needed for dynamic parameter identification
% The computation of regressor matrix is obtained using screw
% theory methods.
% -------------------------------------------------------------------

% Trajectory parameters
N = traj_par.N;
wf = traj_par.wf;
T = traj_par.T;
t = traj_par.t;

% As paramters of the trajectory are in a signle vector we reshape them as
% to feed the function that computes the trajectory
ab = reshape(opt_vars,[12,N]);
a = ab(1:6,:); % sin coeffs
b = ab(7:12,:); % cos coeffs

% To guarantee that positions, velocities and accelerations are zero in the
% beginning and at time T, we add fifth order polynomial to fourier
% series. The parameters of the polynomial depends on the parameters of
% fourier series. Here we compute them.
c_pol = getPolCoeffs(T, a, b, wf, N, traj_par.q0);

% Compute trajectory (Fouruer series + fifth order polynomail)
[q,qd,q2d] = mixed_traj(t, c_pol, a, b, wf, N);

% Obtain observation matrix by computing regressor for each sampling time
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
W = [];
for i = 1:length(t)
%     Y = base_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i));
    if baseQR.motorDynamicsIncluded
        Y = [regressorWithMotorDynamics(q(:,i),qd(:,i),q2d(:,i))*E1, ...
             frictionRegressor(qd(:,i))];
    else
        Y = [full_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i))*E1, ...
             frictionRegressor(qd(:,i))];
    end
    W = vertcat(W,Y);
end

out = cond(W);


function c = plus(a,b)
% function C=plus(A,B)
%
% DESCRIPTION
%   Add two polynomials and combine common terms.
%
% INPUTS
%   A,B: polynomials
%
% OUTPUTS
%   C: polynomial, the result of addition A+B
%
% SYNTAX
%   C= A+B
%     Adds A and B.  A and B must have the same dimensions unless one is
%     is a scalar.  A scalar can be added to anything.
%   C = plus(A,B)
%     Function-call form for addition.

% 6/7/2002: PJS  Initial Coding
% 6/8/2002: PJS  Allow matrices of polynomials

% Promote a to polynomial
a = polynomial(a);
sza = size(a);

% Promote b to polynomial
b = polynomial(b);
szb = size(b);

if isempty(a) || isempty(b)

    if isempty(a) && all(szb==[1 1])
        % empty+scalar = empty(sza)
        c=polynomial(zeros(sza));
        return;
    elseif isempty(b) && all(sza==[1 1])
        % scalar+empty = empty(szb)
        c=polynomial(zeros(szb));
        return;
    elseif all(sza==szb)
        c=polynomial(zeros(sza));
        return;
    else
        error('Matrix dimensions must agree.');
    end

elseif all(sza==szb)
    % Matrix + Matrix

    % Get Dimensions
    nta = size(a.degmat,1);
    nva = length(a.varname);
    ntb = size(b.degmat,1);
    nvb = length(b.varname);

    % Stack up coefficients
    acoef = a.coefficient;
    bcoef = b.coefficient;
    coefficient = [acoef; bcoef];

    if nva==0 && nvb==0
        degmat = zeros(nta+ntb,0);
        varname = {};
    else
        adeg = a.degmat;
        bdeg = b.degmat;
        degmat = blkdiag(adeg,bdeg);
        varname = [a.varname(:); b.varname(:)];
    end

    % Form polynomial and combine terms
    chkval = 0; % skip validity check
    c = polynomial(coefficient,degmat,varname,sza,chkval);
    c = combine(c);

elseif all(sza==[1 1]) || all(szb==[1 1])
    % Scalar + Matrix / Matrix + Scalar

    % Make first term of sum be the scalar.
    if ~all(sza==[1 1])
        temp = a;
        a = b;
        b = temp;
        szb = sza;
    end

    % Turn into matrix+matrix
    a.coefficient = repmat(a.coefficient,1,szb(1)*szb(2));
    a.matdim = szb;
    c = plus(a,b);
else
    error('Matrix dimensions must agree');
end


function need = inits ( os, nos, eta )

%*****************************************************************************80
%
%% INITS finds the number of Chebyshev terms needed to achieve a given accuracy.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2009
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    FORTRAN90 version by John Burkardt.
%
%  Parameters:
%
%    Input, real OS(NOS), the array of coefficients.
%
%    Input, integer NOS, the number of coefficients.
%
%    Input, real ETA, the requested accuracy.
%    A typical value of ETA is ( EPSILON ( 1.0 ) ) / 10.0.
%
%    Output, integer NEED, the number of terms needed for
%    the given accuracy.
%
  if ( nos < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'INITS - Fatal error!\n' );
    fprintf ( 1, '  The number of coefficients is less than 1.\n' );
    fprintf ( 1, '  NOS = %d\n', nos );
    error ( 'INITS - Fatal error!' );
  end

  need = nos;

  err = 0.0;
  for i = nos : -1 : 1
    err = err + abs ( os(i) );
    if ( eta < err )
      need = i;
      return
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'INITS - Warning!\n' );
  fprintf ( 1, '  The requested accuracy, ETA = %e\n', eta );
  fprintf ( 1, '  is smaller than the Chebyshev coefficients\n' );
  fprintf ( 1, '  can guarantee.\n' );

  return
end


function C=multiDimPolyMatMultiply(A,B)
%%MULTIDIMPOLYMATMULTIPLY Given 2D matrices of multivariate polynomials,
%          multiply the matrices to get a resulting matrix containing
%          multivariate polynomials. The multivariate polynomials
%          themselves are represented as hypermatrices, so the "matrices"
%          of these polynomials are represented by cell arrays of
%          hypermatrices holding the polynomial coefficients.
%
%INPUTS:   A An mXr cell array where each element contains a hypermatrix
%            representing a multivariate polynomials. The format of these
%            hypermatrices is discussed below.
%          B An rXn cell array where each element contains a hypermatrix
%            representing a multivariate polynomial.
%
%OUTPUTS: C The mXn-sized product of the matrices of multivariate
%           polynomials, A and B. C is a cell array where each entry is a
%           hypermatrix of coefficients of the multivariate polynomial.
%
%The coefficients of a hypermatrix representing a multivariate polynomial
%coeffs are arranged such that coeffs(a1,a2,a3...an) corresponds to the
%coefficient of an x1^(a1-1)*x2^(a2-1)*x3^(a3-1)...xn^(an-1) term.  Thus,
%the number of indices coeffs takes is equal to the dimensionality of x
%(not counting singleton dimensions at the end of coeffs). Note that this
%ordering is the reverse that used in the 1D polyval function that is built
%into Matlab. The number of elements for each index in coeffs is the
%maximum order of that dimension +1. The indices of the multivariate
%polynomials in the different elements of the cell arrays must represent
%the same variable.
%
%This just implements the rules for basic matrix multiplication as in
%Chapter 1.11 of [1], using the appropriate functions for multiplying
%(convn) and summing (polySumMultiDim) multivariate polynomials.
%
%EXAMPLE:
%COnsider multiplying a 2X2 matrix of polynomials by a 2X1 vector of
%polynomials.
% A=cell(2,2);
% B=cell(2,1);
% %A multivariate linear polynomial with 3 variables.
% %-3+4*x1+5*x1*x2*x3
% A11=zeros(2,2,2);
% A11(0+1,0+1,0+1)=-3;
% A11(1+1,0+1,0+1)=4;
% A11(1+1,1+1,1+1)=5;
% A{1,1}=A11;
% %Both diagonals equal and just a single third-order term.
% %12*x1^3
% A12=zeros(4,1,1);
% A12(3+1,0+1,0+1)=12;
% A{1,2}=A12;
% A{2,1}=A12;
% %A second order polynomial only with x1 and x3.
% %6+x1^2-2*x3^2
% A22=zeros(3,1,3);
% A22(0+1,0+1,0+1)=6;
% A22(2+1,0+1,0+1)=1;
% A22(0+1,0+1,2+1)=-2;
% A{2,2}=A22;
% %Now the B vector.
% %-6+x1*x2+x1^2
% B1=zeros(2,3);
% B1(0+1,0+1)=-6;
% B1(1+1,1+1)=1;
% B1(2+1,0+1)=1;
% B{1}=B1;
% %-12-4*x1^2*x2^2
% B2=zeros(3,3);
% B2(0+1,0+1)=-12;
% B2(2+1,2+1)=-4;
% B{2}=B2;
% C=multiDimPolyMatMultiply(A,B)
% %One finds that C is a 2X1 cell array where the highest exponent in C{1} is
% %3 and the highest in C{2} is 4. We can evaluate the polynomial at a
% %point. Say
% x=[1;2;3];
% f=@(coeffs)polyValMultiDim(coeffs,x);
% val=cellfun(f,C)
%
%REFERENCES:
%[1] G. H. Golub and C. F. Van Loan, Matrix Computations, 4th ed.
%    Baltimore: Johns Hopkins University Press, 2013.
%
%December 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

m=size(A,1);
r=size(A,2);
n=size(B,2);

C=num2cell(zeros(m,n));

for j=1:n
    for k=1:r
        for i=1:m
           polyProd=convn(A{i,k},B{k,j});
           C{i,j}=polySumMultiDim(C{i,j},polyProd);
        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 [W_sorted, ind_sort, modularity, com_id, com_sizes] = gsp_sort_nodes(W, recursive, self_loops)
%GSP_SORT_NODES Sort nodes using the Louvain clustering method
%   Usage:  [Wout, ind, modularity, com_id, com_sizes] = gsp_sort_nodes(W)
%           [Wout, ind, modularity, com_id, com_sizes] = gsp_sort_nodes(W, recursive)
%           [Wout, ind, modularity, com_id, com_sizes] = gsp_sort_nodes(W, recursive, self_loops)
%
%   Inputs:
%         W         : Weigthed adjacency matrix. Can contain self-loops
%         recursive : Use recursive computation (default: 1)
%         self_loops: Take into account self loops (default: 0)
%
%   Outputs:
%         Wout      : Sorted weighted adjacency matrix
%         ind       : Indices used for sorting:       Wout = W(ind, ind)
%         modularity: Modularity of final clustering used for sorting
%         com_id    : Community ID of each node in the initial W
%         com_sizes : Number of nodes in each community
%
%   gsp_sort_nodes(W) sorts the nodes of the weighted adjacency matrix $W$
%   by clustering them according to the "Louvain" method.
%
%   By default the clustering is recursive and the level of recursion that
%   gives the maximum modularity is kept. The nodes of the graph are sorted
%   according to the clusters given.
%
%   The clustering method is using code given free online by Antoine
%   Scherrer available here:
%   https://perso.uclouvain.be/vincent.blondel/research/louvain.html
%
%   Example:::
%
%         G = gsp_nn_graph([[randn(100, 1)-1; (randn(100, 1)+7)], ...
%           [randn(50, 1); (randn(100,1)-4); (randn(50, 1)*2+8)]]);
%         [Wout, ind, modularity, com_id, com_sizes] = gsp_sort_nodes(G.W);
%         figure; imagesc(Wout); title('sorted adjacency matrix');
%         figure; gsp_plot_signal(G, ind);
%
%
%
%   See also: cluster_jl
%
%   References: blondel2008fast
%

% Author: Vassilis Kalofolias
% Date: June 2016

if nargin < 2
    % Use recursive clustering and pick the level with the highest modularity
    recursive = 1;
end
if nargin < 3
    % Don't use self loops by default
    self_loops = 0;
end
debug = 0;
verbose = 0;

COMTY = cluster_jl(W, recursive, self_loops, debug, verbose);

% Keep as best the splitting with highest modularity
ind_best_splitting = argmax(COMTY.MOD);

% Which node belongs to which community
com_id = COMTY.COM{ind_best_splitting};

% Sort according to the communities
[~, ind_sort] = sort(com_id);

modularity = COMTY.MOD(ind_best_splitting);
com_sizes = COMTY.SIZE{ind_best_splitting};

W_sorted = W(ind_sort, ind_sort);


function value = base_to_i4 ( s, base )

%*****************************************************************************80
%
%% BASE_TO_I4 returns the value of an integer represented in some base.
%
%  Discussion:
%
%    BASE = 1 is allowed, in which case we allow the digits '1' and '0',
%    and we simply count the '1' digits for the result.
%
%    Negative bases between -16 and -2 are allowed.
%
%    The base -1 is allowed, and essentially does a parity check on
%    a string of 1's.
%
%  Example:
%
%        Input      Output
%    -------------  ------
%         S   BASE       I
%    ------  -----  ------
%      '101'     2       5
%    '-1000'     3     -27
%      '100'     4      16
%   '111111'     2      63
%   '111111'    -2      21
%   '111111'     1       6
%   '111111'    -1       0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string.  The elements of S are
%    blanks, a plus or minus sign, and digits.  Normally, the digits
%    are representations of integers between 0 and |BASE-1|.  In the
%    special case of base 1 or base -1, we allow both 0 and 1 as digits.
%
%    Input, integer BASE, the base in which the representation is given.
%    Normally, 2 <= BASE <= 16.  However, there are two exceptions.
%
%    Output, integer VALUE, the integer.
%
  value = 0;
  s_length = s_len_trim ( s );

  if ( base == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BASE_TO_I4 - Serious error!\n' );
    fprintf ( 1, '  The input base is zero.\n' );
    error ( 'BASE_TO_I4 - Serious error!' )
  end

  if ( 16 < abs ( base ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BASE_TO_I4 - Serious error!\n' );
    fprintf ( 1, '  The input base is greater than 16!\n' );
    error ( 'BASE_TO_I4 - Serious error!' )
  end

  state = 0;
  isgn = 1 ;
  ichr = 1;

  while ( ichr <= s_length )

    ch = s(ichr);
%
%  Blank.
%
    if ( ch == ' ' )

      if ( state == 2 )
        break
      end
%
%  Sign, + or -.
%
    elseif ( ch == '-' )

      if ( state ~= 0 )
        break
      end

      state = 1;
      isgn = -1;

    elseif ( ch == '+' )

      if ( state ~= 0 )
        break
      end

      state = 1;

    else
%
%  Digit?
%
      digit = hex_digit_to_i4 ( ch );

      if ( abs ( base ) == 1 && ( digit == 0 || digit == 1 ) )

        value = base * value + digit;
        state = 2;

      elseif ( 0 <= digit && digit < abs ( base ) )

        value = base * value + digit;
        state = 2;

      else

        fprintf ( 1, '\n' );
        fprintf ( 1, 'BASE_TO_I4 - Serious error!\n' );
        fprintf ( 1, '  Illegal digit = "%c"\n', ch );
        fprintf ( 1, '  Conversion halted prematurely!\n' );
        error ( 'BASE_TO_I4 - Serious error!' )

      end

    end

    ichr = ichr + 1;

  end
%
%  Once we're done reading information, we expect to be in state 2.
%
  if ( state ~= 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BASE_TO_I4 - Serious error!\n' );
    fprintf ( 1, '  Unable to decipher input!\n' );
    error ( 'BASE_TO_I4 - Serious error!' )
  end
%
%  Account for the sign.
%
  value = isgn * value;

  return
end


function A = genErdosRenyiGraph(N,pConnect)
%%GENERDOSRENYIGRAPH Generates an instance of the Erdös-Renyi model
%                    described in [1-3].
%
%INPUT:
% N: The number of nodes used for generating the graph.
% pConnect: The probability of any chosen edge being realized.
%
%OUTPUT:
% A: The adjacency matrix for the final graph.
%
%EXAMPLE: Generates an instance of the Erdös-Renyi model.
% N = 30;
% pConnect = 0.3;
% A = genErdosRenyiGraph(N,rewireProb);
% g = graph(A);
% plot(g,"MarkerSize",degree(g))
%
%REFERENCES:
%[1] P. Erdos, A. Renyi, et al., "On the evolution of random graphs,"
%    Publ. Math. Inst. Hung. Acad. Sci, vol. 5, no. 1, pp. 17-60, 1960.
%[2] P. Erdos and A. Renyi, "On random graphs i," Publ. Math. Debrecen,
%    vol. 6, pp. 290-297, 1959.
%[3] E. N. Gilbert, "Random graphs," The Annals of Mathematical
%    Statistics, vol. 30, no. 4, pp. 1141-1144, 1959.
%
%August 2022 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

A = zeros(N);
for i = 1:N-1
    for j = i+1:N
        A(i,j) = rand()<pConnect;
    end
end
A = A+A';
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 [cvx_optval,P,q,r,X,lambda] = cheb(A,b,Sigma);

% Computes Chebyshev lower bounds on probability vectors
%
% Calculates a lower bound on the probability that a random vector
% x with mean zero and covariance Sigma satisfies A x <= b
%
% Sigma must be positive definite
%
% output arguments:
% - prob: lower bound on probability
% - P,q,r: x'*P*x + 2*q'*x + r is a quadratic function
%   that majorizes the 0-1 indicator function of the complement
%   of the polyhedron,
% - X, lambda:  a discrete distribution with mean zero, covariance
%   Sigma and Prob(X not in C)  >= 1-prob

%
% maximize  1 - Tr Sigma*P - r
% s.t.      [ P  q     ]             [ 0      a_i/2 ]
%           [ q' r - 1 ] >= tau(i) * [ a_i'/2  -b_i ], i=1,...,m
%           taui >= 0
%           [ P q  ]
%           [ q' r ] >= 0
%
% variables P in Sn, q in Rn, r in R
%

[ m, n ] = size( A );
cvx_begin sdp quiet
    variable P(n,n) symmetric
    variables q(n) r tau(m)
    dual variables Z{m}
    maximize( 1 - trace( Sigma * P ) - r )
    subject to
        for i = 1 : m,
            qadj = q - 0.5 * tau(i) * A(i,:)';
            radj = r - 1 + tau(i) * b(i);
            [ P, qadj ; qadj', radj ] >= 0 : Z{i};
        end
        [ P, q ; q', r ] >= 0;
        tau >= 0;
cvx_end

if nargout < 4,
    return
end

X = [];
lambda = [];
for i=1:m
   Zi = Z{i};
   if (abs(Zi(3,3)) > 1e-4)
      lambda = [lambda; Zi(3,3)];
      X = [X Zi(1:2,3)/Zi(3,3)];
   end;
end;
mu = 1-sum(lambda);
if (mu>1e-5)
   w = (-X*lambda)/mu;
   W = (Sigma - X*diag(lambda)*X')/mu;
   [v,d] = eig(W-w*w');
   d = diag(d);
   s = sum(d>1e-5);
   if (d(1) > 1e-5)
      X = [X w+sqrt(s)*sqrt(d(1))*v(:,1) ...
            w-sqrt(s)*sqrt(d(1))*v(:,1)];
      lambda = [lambda; mu/(2*s); mu/(2*s)];
   elseif (d(2) > 1e-5)
      X = [X w+sqrt(s)*sqrt(d(2))*v(:,2) ...
            w-sqrt(s)*sqrt(d(2))*v(:,2)];
      lambda = [lambda; mu/(2*s); mu/(2*s)];
   else
      X = [X w];
      lambda = [lambda; mu];
   end;
end;


function [x,f,exitflag,output] = minFunc(funObj,x0,options,varargin)
% minFunc(funObj,x0,options,varargin)
%
% Unconstrained optimizer using a line search strategy
%
% Uses an interface very similar to fminunc
%   (it doesn't support all of the optimization toolbox options,
%       but supports many other options).
%
% It computes descent directions using one of ('Method'):
%   - 'newton': Newton
%       (uses user-supplied Hessian matrix)
%   - 'bfgs': Quasi-Newton with BFGS Updating
%       (uses dense Hessian approximation)
%   - 'lbfgs': Quasi-Newton with Limited-Memory BFGS Updating
%       (default: uses a predetermined nunber of previous steps)
%   - 'newton0': Hessian-Free Newton
%       (numerically computes Hessian-Vector products)
%   - 'newton0lbfgs': Hessian-Free Newton with LBFGS Preconditioner
%       (uses predetermined number of previous steps
%           and numerically computes Hessian-vector products)
%   - 'cg': Non-Linear Conjugate Gradient
%       (uses only previous step and a vector beta)
%   - 'bb': Barzilai and Borwein Gradient
%       (uses only previous step)
%   - 'sd': Steepest Descent
%       (no previous information used, not recommended)
%   - 'tensor': Tensor
%       (uses user-supplied Hessian matrix and Tensor of 3rd partial derivatives)
%
% Several line search strategies are available for finding a step length satisfying
%   the termination criteria ('LS'):
%   - 0: Backtrack w/ Step Size Halving
%   - 1: Backtrack w/ Quadratic/Cubic Interpolation from new function values
%   - 2: Backtrack w/ Cubic Interpolation from new function + gradient
%   values (default for 'bb' and 'sd')
%   - 3: Bracketing w/ Step Size Doubling and Bisection
%   - 4: Bracketing w/ Cubic Interpolation/Extrapolation with function +
%   gradient values (default for all except 'bb' and 'sd')
%   - 5: Bracketing w/ Mixed Quadratic/Cubic Interpolation/Extrapolation
%   - 6: Use Matlab Optimization Toolbox's line search
%           (requires Matlab's linesearch.m to be added to the path)
%
%   Above, the first three find a point satisfying the Armijo conditions,
%   while the last four search for find a point satisfying the Wolfe
%   conditions.  If the objective function overflows, it is recommended
%   to use one of the first 3.
%   The first three can be used to perform a non-monotone
%   linesearch by changing the option 'Fref'.
%
% Several strategies for choosing the initial step size are avaiable ('LS_init'):
%   - 0: Always try an initial step length of 1 (default for all except 'cg' and 'sd')
%       (t = 1)
%   - 1: Use a step similar to the previous step (default for 'cg' and 'sd')
%       (t = t_old*min(2,g'd/g_old'd_old))
%   - 2: Quadratic Initialization using previous function value and new
%   function value/gradient (use this if steps tend to be very long)
%       (t = min(1,2*(f-f_old)/g))
%   - 3: The minimum between 1 and twice the previous step length
%       (t = min(1,2*t)
%   - 4: The scaled conjugate gradient step length (may accelerate
%   conjugate gradient methods, but requires a Hessian-vector product)
%       (t = g'd/d'Hd)
%
% Inputs:
%   funObj is a function handle
%   x0 is a starting vector;
%   options is a struct containing parameters
%  (defaults are used for non-existent or blank fields)
%   all other arguments are passed to funObj
%
% Outputs:
%   x is the minimum value found
%   f is the function value at the minimum found
%   exitflag returns an exit condition
%   output returns a structure with other information
%
% Supported Input Options
%   Display - Level of display [ off | final | (iter) | full | excessive ]
%   MaxFunEvals - Maximum number of function evaluations allowed (1000)
%   MaxIter - Maximum number of iterations allowed (500)
%   TolFun - Termination tolerance on the first-order optimality (1e-5)
%   TolX - Termination tolerance on X (1e-9)
%   Method - [ newton | bfgs | {lbfgs} | newton0 | cg | bb | steepdesc ]
%   c1 - Sufficient Decrease for Armijo condition (1e-4)
%   c2 - Curvature Decrease for Wolfe conditions (.2 for cg, .9 otherwise)
%   LS_init - Line Search Initialization -see above (1 for cg/sd, 0 otherwise)
%   LS - Line Search type -see above (2 for bb/sd, 4 otherwise)
%   Fref - Setting this to a positive integer greater than 1
%       will use non-monotone Armijo objective in the line search.
%       (10 for 'bb', 1 for all others)
%   numDiff - compute derivative numerically
%       (default: 0) (this option has a different effect for 'newton', see below)
%   useComplex - if 1, use complex differentials when computing numerical derivatives
%       to get very accurate values (default: 0, objective function must support complex inputs)
%   DerivativeCheck - if 'on', computes derivatives numerically at initial
%   point and compares to user-supplied derivative (default: 'off')
%
% Method-specific input options:
%   newton:
%       HessianModify - type of Hessian modification for direct solvers to
%       use if the Hessian is not positive definite (default: 0)
%           0: Minimum Euclidean norm s.t. eigenvalues sufficiently large
%           (requires eigenvalues on iterations where matrix is not pd)
%           1: Start with (1/2)*||A||_F and increment until Cholesky succeeds
%           (an approximation to method 0, does not require eigenvalues)
%           2: Modified LDL factorization
%           (only 1 generalized Cholesky factorization done and no eigenvalues required)
%           3: Modified Spectral Decomposition
%           (requires eigenvalues)
%           4: Modified Symmetric Indefinite Factorization
%       cgSolve - use conjugate gradient instead of direct solver (default: 0)
%           0: Direct Solver
%           1: Conjugate Gradient
%           2: Conjugate Gradient with Diagonal Preconditioner
%           3: Conjugate Gradient with LBFGS Preconditioner
%           x: Conjugate Graident with Symmetric Successive Over Relaxation
%           Preconditioner with parameter x
%               (where x is a real number in the range [0,2])
%           x: Conjugate Gradient with Incomplete Cholesky Preconditioner
%           with drop tolerance -x
%               (where x is a real negative number)
%       numDiff - compute Hessian numerically
%                 (default: 0, done with complex differentials if useComplex = 1)
%       LS_saveHessiancomp - when on, only computes the Hessian at the
%       first and last iteration of the line search (default: 1)
%   newton0:
%       HvFunc - user-supplied function that returns Hessian-vector products
%           (by default, these are computed numerically using autoHv)
%           HvFunc should have the following interface: HvFunc(v,x,varargin{:})
%       useComplex - use a complex perturbation to get high accuracy
%           Hessian-vector products (default: 0)
%           (the increased accuracy can make the method much more efficient,
%               but gradient code must properly support complex inputs)
%   bfgs:
%       initialHessType - scale initial Hessian approximation (default: 1)
%       SR1 - use SR1 instead of BFGS when it maintains positive definiteness (default: 0)
%       Damped - use damped update (default: 1)
%   lbfgs:
%       Corr - number of corrections to store in memory (default: 100)
%           (higher numbers converge faster but use more memory)
%   cg:
%       cgUpdate - type of update (default: 1)
%           0: Fletcher Reeves
%           1: Polak-Ribiere
%           2: Hestenes-Stiefel
%   bb:
%       bbType - type of bb step (default: 1)
%           0: min_alpha ||delta_x - alpha delta_g||_2
%           1: min_alpha ||alpha delta_x - delta_g||_2
%
% Supported Output Options
%   iterations - number of iterations taken
%   funcCount - number of function evaluations
%   algorithm - algorithm used
%   firstorderopt - first-order optimality
%   message - exit message
%   trace.funccount - function evaluations after each iteration
%   trace.fval - function value after each iteration
%
% Author: Mark Schmidt (2006)
% Web: http://www.cs.ubc.ca/~schmidtm
%
% Sources (in order of how much the source material contributes):
%   J. Nocedal and S.J. Wright.  1999.  "Numerical Optimization".  Springer Verlag.
%   R. Fletcher.  1987.  "Practical Methods of Optimization".  Wiley.
%   J. Demmel.  1997.  "Applied Linear Algebra.  SIAM.
%   R. Barret, M. Berry, T. Chan, J. Demmel, J. Dongarra, V. Eijkhout, R.
%   Pozo, C. Romine, and H. Van der Vost.  1994.  "Templates for the Solution of
%   Linear Systems: Building Blocks for Iterative Methods".  SIAM.
%   J. More and D. Thuente.  "Line search algorithms with guaranteed
%   sufficient decrease".  ACM Trans. Math. Softw. vol 20, 286-307, 1994.
%   M. Raydan.  "The Barzilai and Borwein gradient method for the large
%   scale unconstrained minimization problem".  SIAM J. Optim., 7, 26-33,
%   (1997).
%   "Mathematical Optimization".  The Computational Science Education
%   Project.  1995.
%   C. Kelley.  1999.  "Iterative Methods for Optimization".  Frontiers in
%   Applied Mathematics.  SIAM.

if nargin < 3
    options = [];
end

% Get Parameters
[verbose,verboseI,debug,doPlot,maxFunEvals,maxIter,tolFun,tolX,method,...
    corrections,c1,c2,LS_init,LS,cgSolve,SR1,cgUpdate,initialHessType,...
    HessianModify,Fref,useComplex,numDiff,LS_saveHessianComp,...
    DerivativeCheck,Damped,HvFunc,bbType,cycle,boundStepLength,...
    HessianIter,outputFcn] = ...
    minFunc_processInputOptions(options);

% Constants
SD = 0;
CSD = 1;
CG = 2;
BB = 3;
LBFGS = 4;
BFGS = 5;
NEWTON0 = 6;
NEWTON = 7;
TENSOR = 8;

% Initialize
p = length(x0);
d = zeros(p,1);
x = x0;
t = 1;

% Test Presence of Mex Files
if exist('lbfgsC','file')==3
    lbfgsDir = @lbfgsC;
else
    lbfgsDir = @lbfgs;
end
if exist('mcholC','file')==3
    mcholF = @mcholC;
else
    mcholF = @mchol;
end

% If necessary, form numerical differentiation functions
funEvalMultiplier = 1;
if numDiff && method ~= TENSOR
    varargin(3:end+2) = varargin(1:end);
    varargin{1} = useComplex;
    varargin{2} = funObj;
    if method ~= NEWTON
        if debug
            if useComplex
                fprintf('Using complex differentials for gradient computation\n');
            else
                fprintf('Using finite differences for gradient computation\n');
            end
        end
        funObj = @autoGrad;
    else
        if debug
            if useComplex
                fprintf('Using complex differentials for gradient computation\n');
            else
                fprintf('Using finite differences for gradient computation\n');
            end
        end
        funObj = @autoHess;
    end

    if method == NEWTON0 && useComplex == 1
        if debug
            fprintf('Turning off the use of complex differentials\n');
        end
        useComplex = 0;
    end

    if useComplex
        funEvalMultiplier = p;
    else
        funEvalMultiplier = p+1;
    end
end

% Evaluate Initial Point
if method < NEWTON
    [f,g] = funObj(x,varargin{:});
else
    [f,g,H] = funObj(x,varargin{:});
    computeHessian = 1;
end
funEvals = 1;

if strcmp(DerivativeCheck,'on')
    if numDiff
        fprintf('Can not do derivative checking when numDiff is 1\n');
    end
    % Check provided gradient/hessian function using numerical derivatives
    fprintf('Checking Gradient:\n');
    [f2,g2] = autoGrad(x,useComplex,funObj,varargin{:});

    fprintf('Max difference between user and numerical gradient: %f\n',max(abs(g-g2)));
    if max(abs(g-g2)) > 1e-4
        fprintf('User NumDif:\n');
        [g g2]
        diff = abs(g-g2)
        pause;
    end

    if method >= NEWTON
        fprintf('Check Hessian:\n');
        [f2,g2,H2] = autoHess(x,useComplex,funObj,varargin{:});

        fprintf('Max difference between user and numerical hessian: %f\n',max(abs(H(:)-H2(:))));
        if max(abs(H(:)-H2(:))) > 1e-4
            H
            H2
            diff = abs(H-H2)
            pause;
        end
    end
end

% Output Log
if verboseI
    fprintf('%10s %10s %15s %15s %15s\n','Iteration','FunEvals','Step Length','Function Val','Opt Cond');
end

% Output Function
if ~isempty(outputFcn)
    callOutput(outputFcn,x,'init',0,funEvals,f,[],[],g,[],sum(abs(g)),varargin{:});
end

% Initialize Trace
trace.fval = f;
trace.funcCount = funEvals;

% Check optimality of initial point
if sum(abs(g)) <= tolFun
    exitflag=1;
    msg = 'Optimality Condition below TolFun';
    if verbose
        fprintf('%s\n',msg);
    end
    if nargout > 3
        output = struct('iterations',0,'funcCount',1,...
            'algorithm',method,'firstorderopt',sum(abs(g)),'message',msg,'trace',trace);
    end
    return;
end

% Perform up to a maximum of 'maxIter' descent steps:
for i = 1:maxIter

    % ****************** COMPUTE DESCENT DIRECTION *****************

    switch method
        case SD % Steepest Descent
            d = -g;

        case CSD % Cyclic Steepest Descent

            if mod(i,cycle) == 1 % Use Steepest Descent
                alpha = 1;
                LS_init = 1;
                LS = 4; % Precise Line Search
            elseif mod(i,cycle) == mod(1+1,cycle) % Use Previous Step
                alpha = t;
                LS_init = 0;
                LS = 2; % Non-monotonic line search
            end
            d = -alpha*g;

        case CG % Non-Linear Conjugate Gradient

            if i == 1
                d = -g; % Initially use steepest descent direction
            else
                gtgo = g'*g_old;
                gotgo = g_old'*g_old;

                if cgUpdate == 0
                    % Fletcher-Reeves
                    beta = (g'*g)/(gotgo);
                elseif cgUpdate == 1
                    % Polak-Ribiere
                    beta = (g'*(g-g_old)) /(gotgo);
                elseif cgUpdate == 2
                    % Hestenes-Stiefel
                    beta = (g'*(g-g_old))/((g-g_old)'*d);
                elseif cgUpdate == 3
                    % Gilbert-Nocedal
                    beta_FR = (g'*(g-g_old)) /(gotgo);
                    beta_PR = (g'*g-gtgo)/(gotgo);
                    beta = max(-beta_FR,min(beta_PR,beta_FR));
                end
                d = -g + beta*d;

                % Restart if beta is negative, the gradients are far
                % from mutually orthogonal, or the directional deriv is positive
                if beta < 0 || abs(gtgo)/(gotgo) >= 0.1 || g'*d >= 0
                    if debug
                        fprintf('Restarting CG\n');
                    end
                    beta = 0;
                    d = -g;
                end

            end
            g_old = g;

        case BB % Steepest Descent with Barzilai and Borwein Step Length

            if i == 1
                d = -g;
            else
                y = g-g_old;
                s = t*d;
                if bbType == 0
                    yy = y'*y;
                    alpha = (s'*y)/(yy);
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                elseif bbType == 1
                    sy = s'*y;
                    alpha = (s'*s)/sy;
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                elseif bbType == 2 % Conic Interpolation ('Modified BB')
                    sy = s'*y;
                    ss = s'*s;
                    alpha = ss/sy;
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                    alphaConic = ss/(6*(myF_old - f) + 4*g'*s + 2*g_old'*s);
                    if alphaConic > .001*alpha && alphaConic < 1000*alpha
                        alpha = alphaConic;
                    end
                elseif bbType == 3 % Gradient Method with retards (bb type 1, random selection of previous step)
                    sy = s'*y;
                    alpha = (s'*s)/sy;
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                    v(1+mod(i-2,5)) = alpha;
                    alpha = v(ceil(rand*length(v)));
                end
                d = -alpha*g;
            end
            g_old = g;
            myF_old = f;

        case LBFGS % L-BFGS

            % Update the direction and step sizes

            if i == 1
                d = -g; % Initially use steepest descent direction
                old_dirs = zeros(length(g),0);
                old_stps = zeros(length(d),0);
                Hdiag = 1;
            else
                if Damped
                    [old_dirs,old_stps,Hdiag] = dampedUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                else
                    [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                end
                d = lbfgsDir(-g,old_dirs,old_stps,Hdiag);
            end
            g_old = g;

        case BFGS % Use BFGS Hessian approximation

            if i == 1

                % Initially use steepest descent direction
                d = -g;
            else

                y = g-g_old;
                s = t*d;

                if i == 2
                    % Set Initial Hessian approximation

                    if initialHessType == 0
                        % Identity
                        R = eye(length(g));
                    else
                        % Scaled Identity
                        if debug
                            fprintf('Scaling Initial Hessian Approximation\n');
                        end
                        R = sqrt((y'*y)/(y'*s))*eye(length(g));
                    end
                end

                if SR1
                    % Perform SR1 Update if it maintains positive-definiteness
                    ymBs = y-R'*R*s;

                    if sum(abs(s'*ymBs)) >= sum(abs(s))*sum(abs(ymBs))*1e-8
                        [R,posDef] = cholupdate(R,-ymBs/sqrt(ymBs'*s),'-');
                        if posDef ~= 0
                            % Do positive definite BFGS update
                            if debug
                                fprintf('SR1 not positive-definite, doing BFGS Update\n');
                            end
                            if Damped
                                eta = .02;
                                % Todo: change the below to use matrix
                                % vector products
                                B = R'*R;
                                if y'*s < eta*s'*B*s
                                    if debug
                                        fprintf('Damped Update\n');
                                    end
                                    theta = min(max(0,((1-eta)*s'*B*s)/(s'*B*s - y'*s)),1);
                                    y = theta*y + (1-theta)*B*s;
                                end
                                [R,posDef]= cholupdate(cholupdate(R,y/sqrt(y'*s)),R'*R*s/sqrt(s'*R'*R*s),'-');
                            else
                                if y'*s > 1e-10
                                    [R,posDef]= cholupdate(cholupdate(R,y/sqrt(y'*s)),R'*R*s/sqrt(s'*R'*R*s),'-');
                                else
                                    if debug
                                        fprintf('Skipping Update\n');
                                    end
                                end
                            end
                        end
                    end
                else %BFGS

                    % We are doing the rank-2 update to the Hessian approximation B:
                    % B = B + (y*y')/(y'*s) - (B*s*s'*B)/(s'*B*s);

                    if Damped
                        eta = .02;
                        B = R'*R;
                        if y'*s < eta*s'*B*s
                            if debug
                                fprintf('Damped Update\n');
                            end
                            theta = min(max(0,((1-eta)*s'*B*s)/(s'*B*s - y'*s)),1);
                            y = theta*y + (1-theta)*B*s;
                        end
                        [R,posDef]= cholupdate(cholupdate(R,y/sqrt(y'*s)),R'*R*s/sqrt(s'*R'*R*s),'-');
                    else
                        if y'*s > 1e-10
                            [R,posDef]= cholupdate(cholupdate(R,y/sqrt(y'*s)),R'*R*s/sqrt(s'*R'*R*s),'-');
                        else
                            if debug
                                fprintf('Skipping Update\n');
                            end
                        end
                    end
                    % =====================================================
                    % An alternate to the above is to use an approximation of
                    % the inverse Hessian:
                    % R = (eye(p,p) - ((s*y')/(y'*s)))*R*(eye(p,p) - ((y*s')/(y'*s))) + (s*s')/(y'*s);
                    % d = -R*g;
                    % ====================================================
                end

                d = -R\(R'\g);
            end

            g_old = g;

        case NEWTON0 % Hessian-Free Newton

            cgMaxIter = min(p,maxFunEvals-funEvals);
            cgForce = min(0.5,sqrt(norm(g)))*norm(g);

            % Set-up preconditioner
            precondFunc = [];
            precondArgs = [];
            if cgSolve == 1
                if i == 1
                    old_dirs = zeros(length(g),0);
                    old_stps = zeros(length(g),0);
                    Hdiag = 1;
                else
                    [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                    precondFunc = lbfgsDir;
                    precondArgs = {old_dirs,old_stps,Hdiag};
                end
                g_old = g;
            end

            % Solve Newton system using cg and hessian-vector products
            if isempty(HvFunc)
                % No user-supplied Hessian-vector function,
                % use automatic differentiation
                HvArgs = {x,g,useComplex,funObj,varargin{:}};
                [d,cgIter,cgRes] = conjGrad([],-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs,@autoHv,HvArgs);
            else
                % Use user-supplid Hessian-vector function
                HvArgs = {x,varargin{:}};
                [d,cgIter,cgRes] = conjGrad([],-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs,HvFunc,HvArgs);
            end

            funEvals = funEvals+cgIter;
            if debug
                fprintf('newtonCG stopped on iteration %d w/ residual %.5e\n',cgIter,cgRes);
            end

        case NEWTON % Newton search direction

            if cgSolve == 0
                if HessianModify == 0
                    % Attempt to perform a Cholesky factorization of the Hessian
                    [R,posDef] = chol(H);

                    % If the Cholesky factorization was successful, then the Hessian is
                    % positive definite, solve the system
                    if posDef == 0
                        d = -R\(R'\g);

                    else
                        % otherwise, adjust the Hessian to be positive definite based on the
                        % minimum eigenvalue, and solve with QR
                        % (expensive, we don't want to do this very much)
                        if debug
                            fprintf('Adjusting Hessian\n');
                        end
                        H = H + eye(length(g)) * max(0,1e-12 - min(real(eig(H))));
                        d = -H\g;
                    end
                elseif HessianModify == 1
                    % Modified Incomplete Cholesky
                    R = mcholinc(H,debug);
                    d = -R\(R'\g);
                elseif HessianModify == 2
                    % Modified Generalized Cholesky
                    [L D perm] = mcholF(H);
                    d(perm) = -L' \ ((D.^-1).*(L \ g(perm)));
                elseif HessianModify == 3
                    % Modified Spectral Decomposition
                    [V,D] = eig((H+H')/2);
                    D = diag(D);
                    D = max(abs(D),max(max(abs(D)),1)*1e-12);
                    d = -V*((V'*g)./D);
                else
                    % Modified Symmetric Indefinite Factorization
                    [L,D,perm] = ldl(H,'vector');
                    [blockPos junk] = find(triu(D,1));
                    for diagInd = setdiff(setdiff(1:p,blockPos),blockPos+1)
                        if D(diagInd,diagInd) < 1e-12
                            D(diagInd,diagInd) = 1e-12;
                        end
                    end
                    for blockInd = blockPos'
                        block = D(blockInd:blockInd+1,blockInd:blockInd+1);
                        block_a = block(1);
                        block_b = block(2);
                        block_d = block(4);
                        lambda = (block_a+block_d)/2 - sqrt(4*block_b^2 + (block_a - block_d)^2)/2;
                        D(blockInd:blockInd+1,blockInd:blockInd+1) = block+eye(2)*(lambda+1e-12);
                    end
                    d(perm) = -L' \ (D \ (L \ g(perm)));
                end

            else
                % Solve with Conjugate Gradient
                cgMaxIter = p;
                cgForce = min(0.5,sqrt(norm(g)))*norm(g);

                % Select Preconditioner
                if cgSolve == 1
                    % No preconditioner
                    precondFunc = [];
                    precondArgs = [];
                elseif cgSolve == 2
                    % Diagonal preconditioner
                    precDiag = diag(H);
                    precDiag(precDiag < 1e-12) = 1e-12 - min(precDiag);
                    precondFunc = @precondDiag;
                    precondArgs = {precDiag.^-1};
                elseif cgSolve == 3
                    % L-BFGS preconditioner
                    if i == 1
                        old_dirs = zeros(length(g),0);
                        old_stps = zeros(length(g),0);
                        Hdiag = 1;
                    else
                        [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                    end
                    g_old = g;
                    precondFunc = lbfgsDir;
                    precondArgs = {old_dirs,old_stps,Hdiag};
                elseif cgSolve > 0
                    % Symmetric Successive Overelaxation Preconditioner
                    omega = cgSolve;
                    D = diag(H);
                    D(D < 1e-12) = 1e-12 - min(D);
                    precDiag = (omega/(2-omega))*D.^-1;
                    precTriu = diag(D/omega) + triu(H,1);
                    precondFunc = @precondTriuDiag;
                    precondArgs = {precTriu,precDiag.^-1};
                else
                    % Incomplete Cholesky Preconditioner
                    opts.droptol = -cgSolve;
                    opts.rdiag = 1;
                    R = cholinc(sparse(H),opts);
                    if min(diag(R)) < 1e-12
                        R = cholinc(sparse(H + eye*(1e-12 - min(diag(R)))),opts);
                    end
                    precondFunc = @precondTriu;
                    precondArgs = {R};
                end

                % Run cg with the appropriate preconditioner
                if isempty(HvFunc)
                    % No user-supplied Hessian-vector function
                    [d,cgIter,cgRes] = conjGrad(H,-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs);
                else
                    % Use user-supplied Hessian-vector function
                    [d,cgIter,cgRes] = conjGrad(H,-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs,HvFunc,{x,varargin{:}});
                end
                if debug
                    fprintf('CG stopped after %d iterations w/ residual %.5e\n',cgIter,cgRes);
                    %funEvals = funEvals + cgIter;
                end
            end

        case TENSOR % Tensor Method

            if numDiff
                % Compute 3rd-order Tensor Numerically
                [junk1 junk2 junk3 T] = autoTensor(x,useComplex,funObj,varargin{:});
            else
                % Use user-supplied 3rd-derivative Tensor
                [junk1 junk2 junk3 T] = funObj(x,varargin{:});
            end
            options_sub.Method = 'newton';
            options_sub.Display = 'none';
            options_sub.TolX = tolX;
            options_sub.TolFun = tolFun;
            d = minFunc(@taylorModel,zeros(p,1),options_sub,f,g,H,T);

            if any(abs(d) > 1e5) || all(abs(d) < 1e-5) || g'*d > -tolX
                if debug
                    fprintf('Using 2nd-Order Step\n');
                end
                [V,D] = eig((H+H')/2);
                D = diag(D);
                D = max(abs(D),max(max(abs(D)),1)*1e-12);
                d = -V*((V'*g)./D);
            else
                if debug
                    fprintf('Using 3rd-Order Step\n');
                end
            end
    end

    % ****************** COMPUTE STEP LENGTH ************************

    % Directional Derivative
    gtd = g'*d;

    % Check that progress can be made along direction
    if gtd > -tolX
        exitflag=2;
        msg = 'Directional Derivative below TolX';
        break;
    end

    % Select Initial Guess
    if i == 1
        if method < NEWTON0
            t = min(1,1/sum(abs(g)));
        else
            t = 1;
        end
    else
        if LS_init == 0
            % Newton step
            t = 1;
        elseif LS_init == 1
            % Close to previous step length
            t = t*min(2,(gtd_old)/(gtd));
        elseif LS_init == 2
            % Quadratic Initialization based on {f,g} and previous f
            t = min(1,2*(f-f_old)/(gtd));
        elseif LS_init == 3
            % Double previous step length
            t = min(1,t*2);
        elseif LS_init == 4
            % Scaled step length if possible
            dHd = d'*autoHv(d,x,g,0,funObj,varargin{:});
            funEvals = funEvals + 1;
            if dHd > 0
                t = -gtd/(dHd);
            else
                t = t*min(2,(gtd_old)/(gtd));
            end
        end

        if t <= 0
            t = 1;
        end
    end
    f_old = f;
    gtd_old = gtd;

    % Bound the initial step size
    if boundStepLength && method < NEWTON0
        t = min(t,1e4/(1+sum(abs(g))));
    end

    % Compute reference fr if using non-monotone objective
    if Fref == 1
        fr = f;
    else
        if i == 1
            old_fvals = repmat(-inf,[Fref 1]);
        end

        if i <= Fref
            old_fvals(i) = f;
        else
            old_fvals = [old_fvals(2:end);f];
        end
        fr = max(old_fvals);
    end

    computeHessian = 0;
    if method >= NEWTON
        if HessianIter == 1
            computeHessian = 1;
        elseif i > 1 && mod(i-1,HessianIter) == 0
            computeHessian = 1;
        end
    end

    % Line Search
    f_old = f;
    if LS < 3 % Use Armijo Bactracking
        % Perform Backtracking line search
        if computeHessian
            [t,x,f,g,LSfunEvals,H] = ArmijoBacktrack(x,t,d,f,fr,g,gtd,c1,LS,tolX,debug,doPlot,LS_saveHessianComp,funObj,varargin{:});
        else
            [t,x,f,g,LSfunEvals] = ArmijoBacktrack(x,t,d,f,fr,g,gtd,c1,LS,tolX,debug,doPlot,1,funObj,varargin{:});
        end
        funEvals = funEvals + LSfunEvals;

    elseif LS < 6
        % Find Point satisfying Wolfe

        if computeHessian
            [t,f,g,LSfunEvals,H] = WolfeLineSearch(x,t,d,f,g,gtd,c1,c2,LS,25,tolX,debug,doPlot,LS_saveHessianComp,funObj,varargin{:});
        else
            [t,f,g,LSfunEvals] = WolfeLineSearch(x,t,d,f,g,gtd,c1,c2,LS,25,tolX,debug,doPlot,1,funObj,varargin{:});
        end
        funEvals = funEvals + LSfunEvals;
        x = x + t*d;

    else
        % Use Matlab optim toolbox line search
        [t,f_new,fPrime_new,g_new,LSexitFlag,LSiter]=...
            lineSearch({'fungrad',[],funObj},x,p,1,p,d,f,gtd,t,c1,c2,-inf,maxFunEvals-funEvals,...
            tolFun,[],[],[],varargin{:});
        funEvals = funEvals + LSiter;
        if isempty(t)
            exitflag = -2;
            msg = 'Matlab LineSearch failed';
            break;
        end

        if method >= NEWTON
            [f_new,g_new,H] = funObj(x + t*d,varargin{:});
            funEvals = funEvals + 1;
        end
        x = x + t*d;
        f = f_new;
        g = g_new;
    end

    % Output iteration information
    if verboseI
        fprintf('%10d %10d %15.5e %15.5e %15.5e\n',i,funEvals*funEvalMultiplier,t,f,sum(abs(g)));
    end

    % Output Function
    if ~isempty(outputFcn)
        callOutput(outputFcn,x,'iter',i,funEvals,f,t,gtd,g,d,sum(abs(g)),varargin{:});
    end

    % Update Trace
    trace.fval(end+1,1) = f;
    trace.funcCount(end+1,1) = funEvals;

    % Check Optimality Condition
    if sum(abs(g)) <= tolFun
        exitflag=1;
        msg = 'Optimality Condition below TolFun';
        break;
    end

    % ******************* Check for lack of progress *******************

    if sum(abs(t*d)) <= tolX
        exitflag=2;
        msg = 'Step Size below TolX';
        break;
    end

    if abs(f-f_old) < tolFun
        exitflag=2;
        msg = 'Function Value changing by less than TolFun';
        break;
    end

    % ******** Check for going over iteration/evaluation limit *******************

    if funEvals*funEvalMultiplier > maxFunEvals
        exitflag = 0;
        msg = 'Exceeded Maximum Number of Function Evaluations';
        break;
    end

    if i == maxIter
        exitflag = 0;
        msg='Exceeded Maximum Number of Iterations';
        break;
    end

end

if verbose
    fprintf('%s\n',msg);
end
if nargout > 3
    output = struct('iterations',i,'funcCount',funEvals*funEvalMultiplier,...
        'algorithm',method,'firstorderopt',sum(abs(g)),'message',msg,'trace',trace);
end

% Output Function
if ~isempty(outputFcn)
    callOutput(outputFcn,x,'done',i,funEvals,f,t,gtd,g,d,sum(abs(g)),varargin{:});
end

end


function label = litekmeans(X, k)
% Perform k-means clustering.
%   X: d x n data matrix
%   k: number of seeds
% Written by Michael Chen (sth4nth@gmail.com).
n = size(X,2);
last = 0;
label = ceil(k*rand(1,n));  % random initialization
while any(label ~= last)
    [u,~,label] = unique(label);   % remove empty clusters
    k = length(u);
    E = sparse(1:n,label,1,n,k,n);  % transform label into indicator matrix
    m = X*(E*spdiags(1./sum(E,1)',0,k,k));    % compute m of each cluster
    last = label;
    [~,label] = max(bsxfun(@minus,m'*X,dot(m,m,1)'/2),[],1); % assign samples to the nearest centers
end
[~,~,label] = unique(label);


%
% Smooth point-set registration method using neighboring constraints
% -------------------------------------------------------------------
%
% Authors: Gerard Sanromà, René Alquézar and Francesc Serratosa
%
% Contact: gsanorma@gmail.com
% Date: 15/02/2012
%
% Centers and rescales input graphs according to the coefficients of the
% match matrix. It follows the ansatz by Softassign Procrustes.
%
% Input
%   g_M,g_D: 2D coordinates of the model and data graph nodes
%   S: matrix of matching coefficients
%
% Output
%   g_Mc,g_Dc: centered and rescaled coordinates of the model and data
%              graphs' nodes
%

function [g_Mc g_Dc] = center_rescale_weighted(g_M,g_D,S)
% Softassign Procrustes (Rangarajan)
% Update centroids and variances (eq.(5))
m_M = sum(g_M.*repmat(sum(S)',1,2))/(sum(S(:))+eps);
m_D = sum(g_D.*repmat(sum(S,2),1,2))/(sum(S(:))+eps);
r_M = g_M - repmat(m_M,size(g_M,1),1);
r_D = g_D - repmat(m_D,size(g_D,1),1);
v_M = sum(diag(r_M*r_M').*sum(S)');%/sum(S(:));
v_D = sum(diag(r_D*r_D').*sum(S,2));%/sum(S(:));
% Center and rescale
g_Mc = r_M/(sqrt(v_M)+eps);
g_Dc = r_D/(sqrt(v_D)+eps);


function V_corr = make_vcorr(DD,pv,nb,nl,f,Zb)
%MAKE_VCORR  Voltage Correction used in distribution power flow
%
%   V_corr = make_vcorr(DD,pv,nb,nl,f,Zb)
%
%   Calculates voltage corrections with current generators placed at PV
%   buses. Their currents are calculated with the voltage difference at PV
%   buses break points and loop impedances. The slack bus voltage is set to
%   zero. Details can be seen in
%   D. Rajicic, R. Ackovski and R. Taleski, "Voltage correction power flow,"
%   IEEE Transactions on Power Delivery, vol. 9, no. 2, pp. 1056-1062, Apr 1994.
%   https://doi.org/10.1109/61.296308
%
%   See also RADIAL_PF.

V_corr = zeros(nb,1);
I = zeros(nb,1);
I(pv) = DD;
% backward sweep
for k = nl:-1:2
    i = f(k);
    I(i) = I(i) + I(k);
end
% forward sweep
for k = 2:nl
    i = f(k);
    V_corr(k) = V_corr(i) - Zb(k) * I(k);
end


function [rI, Par]   =  MCWNNM_ADMM1_Denoising( nI, I, Par )
rI           = nI;   % Estimated Image
[h, w, ch]  = size(rI);
Par.h = h;
Par.w = w;
Par.ch = ch;
Par = SearchNeighborIndex( Par );
% noisy image to patch
NoiPat =	Image2Patch( nI, Par );
Par.TolN = size(NoiPat, 2);
Sigma_arrCh = zeros(Par.ch, Par.TolN);
for iter = 1 : Par.Iter
    Par.iter = iter;
    % iterative regularization
    rI =	rI + Par.delta * (nI - rI);
    % image to patch
    CurPat =	Image2Patch( rI, Par );
    % estimate local noise variance
    for c = 1:Par.ch
        if(iter == 1)
            TempSigma_arrCh = sqrt(max(0, repmat(Par.nSig0(c)^2, 1, size(CurPat, 2)) - mean((NoiPat((c-1)*Par.ps2+1:c*Par.ps2, :) - CurPat((c-1)*Par.ps2+1:c*Par.ps2, :)).^2)));
            %             TempSigma_arrCh = sqrt(abs(repmat(Par.nSig0(c)^2, 1, size(CurPat, 2)) - mean((NoiPat((c-1)*Par.ps2+1:c*Par.ps2, :) - CurPat((c-1)*Par.ps2+1:c*Par.ps2, :)).^2)));
        else
            TempSigma_arrCh = Par.lambda*sqrt(max(0, repmat(Par.nSig0(c)^2, 1, size(CurPat, 2)) - mean((NoiPat((c-1)*Par.ps2+1:c*Par.ps2, :) - CurPat((c-1)*Par.ps2+1:c*Par.ps2, :)).^2)));
            %             TempSigma_arrCh = Par.lambda*sqrt(abs(repmat(Par.nSig0(c)^2, 1, size(CurPat, 2)) - mean((NoiPat((c-1)*Par.ps2+1:c*Par.ps2, :) - CurPat((c-1)*Par.ps2+1:c*Par.ps2, :)).^2)));
        end
        Sigma_arrCh((c-1)*Par.ps2+1:c*Par.ps2, :) = repmat(TempSigma_arrCh, [Par.ps2, 1]);
    end
    if (mod(iter-1, Par.Innerloop) == 0)
        Par.nlsp = Par.nlsp - 10;  % Lower Noise level, less NL patches
        NL_mat  =  Block_Matching(CurPat, Par);% Caculate Non-local similar patches for each
    end
    % Denoising by MCWNNM
    [Y_hat, W_hat]  =  MCWNNM_ADMM1_Estimation( NL_mat, Sigma_arrCh, CurPat, Par );   % Estimate all the patches
    rI = PGs2Image(Y_hat, W_hat, Par);
    PSNR  = csnr( I, rI, 0, 0 );
    SSIM      =  cal_ssim( I, rI, 0, 0 );
    fprintf( 'Iter = %2.3f, PSNR = %2.2f, SSIM = %2.2f \n', iter, PSNR, SSIM );
    Par.PSNR(iter, Par.image)  =   PSNR;
    Par.SSIM(iter, Par.image)      =  SSIM;
end
return;


% revised on September 21, 2010 to make parameters consistent

% September 25, 2009
% written by Shuiwang Ji and Jieping Ye

% This function implements the accelerated gradient algorithm for
% multi-task learning regularized by trace norm as
% described in Ji and Ye (ICML 2009).

% References:
%Ji, S. and Ye, J. 2009. An accelerated gradient method for trace norm minimization.
%In Proceedings of the 26th Annual international Conference on Machine Learning
%(Montreal, Quebec, Canada, June 14 - 18, 2009). ICML '09, vol. 382. ACM, New York,
%NY, 457-464.

%[W, fval_vec,itr_counter] =
%accel_grad_mtl(A,Y,lambda,opt)

% required inputs:
% A: K x 1 cell in which each cell is N x D where N is the sample size
% and D is data dimensionality and K is the number of tasks
% Y: K x 1 cell in which each cell contains the output of the
% corresponding task
% lambda: regularization parameter

% optional inputs:
% opt.L0: Initial guess for the Lipschitz constant
% opt.gamma: the multiplicative factor for Lipschitz constant
% opt.W_init: initial weight matrix
% opt.epsilon: precision for termination
% opt.max_itr: maximum number of iterations

% outputs:
% W: the computed weight matrix
% fval_vec: a vector for the sequence of function values
% itr_counter: number of iterations executed

function [W,fval_vec,itr_counter] = accel_grad_mtl(A,Y,lambda,opt)

Xtrain = A;
Ytrain = Y;
clear A;
clear Y;

if nargin<4
    opt = [];
end

if isfield(opt, 'L0')
    L0 = opt.L0;
else
    L0 = 100;
end

if isfield(opt, 'gamma')
    gamma = opt.gamma;
else
    gamma = 1.1;
end

if isfield(opt, 'W_init')
    W_init = opt.W_init;
else
    W_init = zeros(size(Xtrain{1},2),length(Ytrain));
end

if isfield(opt, 'epsilon')
    epsilon = opt.epsilon;
else
    epsilon = 10^-5;
end

if isfield(opt, 'max_itr')
    max_itr = opt.max_itr;
else
    max_itr = 100;
end

W_old = W_init;
alpha = 1;
fval_vec = [];
L = L0;
fval_old = rand(1,1);
fval = rand(1,1);
itr_counter = 0;
Z_old = W_old;
while abs((fval_old-fval)/fval_old)>epsilon
    fval_old = fval;
    [Wp,P,sval] = ComputeQP(Xtrain,Ytrain,Z_old,L,lambda);
    f = 0;
    for i = 1:length(Xtrain)
        f = f+0.5*norm(Ytrain{i}-Xtrain{i}*Wp(:,i))^2;
    end
    fval = f+lambda*sval;
    Q = P+lambda*sval;
    while fval>Q
        fprintf('Searching step size (fval = %f, Q = %f)...\n',fval,Q);
        L = L*gamma;
        [Wp,P,sval] = ComputeQP(Xtrain,Ytrain,Z_old,L,lambda);
        f = 0;
        for i = 1:length(Xtrain)
            f = f+0.5*norm(Ytrain{i}-Xtrain{i}*Wp(:,i))^2;
        end
        fval = f+lambda*sval;
        Q = P+lambda*sval;
    end

    alpha_old = alpha;
    alpha = (1+sqrt(1+4*alpha_old^2))/2;
    Z_old = Wp+((alpha_old-1)/alpha)*(Wp-W_old);

    W_old = Wp;
    fval_vec = [fval_vec,fval];
    itr_counter = itr_counter+1;
    if itr_counter>max_itr
        break;
    end
    if mod(itr_counter,100)==0
        fprintf('Iteration = %8d,  objective = %f\n',itr_counter, fval);
    end

end
W = Wp;
return;

function [Wp,P,sval] = ComputeQP(X,Y,W,L,lambda)

[W1,delta_W] = ComputeGradStep(X,Y,W,L);
[U,D,V] = svd(W1,0);
D = diag(D);
D = D-(lambda/L);
idx = find(D>0);
sval = sum(D(idx));
Wp = U(:,idx)*diag(D(idx))*V(:,idx)';
P = 0;
for i = 1:length(X)
    P = P+0.5*norm(Y{i}-X{i}*W(:,i))^2;
end
P = P+trace(delta_W'*(Wp-W))+0.5*L*norm(Wp-W,'fro')^2;
return;

function [W1,delta_W] = ComputeGradStep(X,Y,W,L)
delta_W = ComputeDerivative(X,Y,W);
W1 = W-(1/L)*delta_W;
return;

function dev = ComputeDerivative(X,Y,W)

dev = [];
for i = 1:length(X)
    dev = [dev, X{i}'*(X{i}*W(:,i)-Y{i})];
end
return;


function [node,elem] = squaremesh(square,h)
%% SQUAREMESH uniform mesh of a square
%
% [node,elem] = squaremesh([x0,x1,y0,y1],h) generates a uniform mesh of the
% rectangle [x0,x1]*[y0,y1] with mesh size h.
%
% Example
%
%   [node,elem] = squaremesh([0,1,0,1],0.2);
%   showmesh(node,elem);
%   findnode(node);
%   findelem(node,elem);
%
% See also: squarequadmesh, cubehexmesh
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Generate nodes
x0 = square(1); x1 = square(2);
y0 = square(3); y1 = square(4);
[x,y] = meshgrid(x0:h:x1,y0:h:y1);
node = [x(:),y(:)];

%% Generate elements
ni = size(x,1); % number of rows
N = size(node,1);
t2nidxMap = 1:N-ni;
topNode = ni:ni:N-ni;
t2nidxMap(topNode) = [];
k = (t2nidxMap)';
elem = [k+ni k+ni+1 k; k+1 k k+ni+1];
% 4 k+1 --- k+ni+1 3
%    |        |
% 1  k  ---  k+ni  2


function G=constructMatrixOfMonomials(g,order)
%-fanDTasia ToolBox------------------------------------------------------------------
% This Matlab script is part of the fanDTasia ToolBox: a Matlab library for Diffusion
% Weighted MRI (DW-MRI) Processing, Diffusion Tensor (DTI) Estimation, High-order
% Diffusion Tensor Analysis, Tensor ODF estimation, Visualization and more.
%
% A Matlab Tutorial on DW-MRI can be found in:
% http://www.cise.ufl.edu/~abarmpou/lab/fanDTasia/tutorial.php
%
%-CITATION---------------------------------------------------------------------------
% If you use this software please cite the following work:
% A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion Tensors
% of any order with Symmetric Positive-Definite Constraints",
% In the Proceedings of ISBI, 2010
%
%-DESCRIPTION------------------------------------------------------------------------
% This function computes all possible monomials in 3 variables of a certain order.
% The 3 variables are given in the form of 3-dimensional vectors.
%
%-USE--------------------------------------------------------------------------------
% G=constructMatrixOfMonomials(g,order);
%
% g: is a list of N 3-dimensional vectors stacked in a matrix of size Nx3
% order: is the order of the computed monomials
% G: contains the computed monomials. It is a matrix of size N x (2+order)!/2(order)!
%
%-DISCLAIMER-------------------------------------------------------------------------
% You can use this source code for non commercial research and educational purposes
% only without licensing fees and is provided without guarantee or warrantee expressed
% or implied. You cannot repost this file without prior written permission from the
% authors. If you use this software please cite the following work:
% A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion Tensors
% of any order with Symmetric Positive-Definite Constraints", In Proc. of ISBI, 2010.
%
%-AUTHOR-----------------------------------------------------------------------------
% Angelos Barmpoutis, PhD
% Computer and Information Science and Engineering Department
% University of Florida, Gainesville, FL 32611, USA
% abarmpou at cise dot ufl dot edu
%------------------------------------------------------------------------------------

fprintf(1,'Constructing matrix of monomials G...');
for k=1:length(g)
    c=1;
    for i=0:order
		for j=0:order-i
			G(k,c)=(g(k,1)^i)*(g(k,2)^j)*(g(k,3)^(order-i-j));
			c=c+1;
        end
    end
end
fprintf(1,'Done\n');


function blend_test04 ( )

%*****************************************************************************80
%
%% BLEND_TEST04 checks out BLEND_IJ_0D1 and BLEND_IJ_1D1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2008
%
%  Author:
%
%    John Burkardt
%
  m1 = 5;
  m2 = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BLEND_TEST04\n' );
  fprintf ( 1, '  BLEND_IJ_0D1 interpolates data in a table,\n' );
  fprintf ( 1, '  from corner data.\n' );
  fprintf ( 1, '  BLEND_IJ_1D1 interpolates data in a table,\n' );
  fprintf ( 1, '  from edge data.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The table is %d rows by %d columns.\n', m1, m2 );

  x = zeros ( m1, m2 );
%
%  Load data in the corners only.
%
  i = 1;
  j = 1;
  r = (  i - 1 ) / ( m1 - 1 );
  s = (  j - 1 ) / ( m2 - 1 );
  x(i,j) = cubic_rs ( r, s, 1 );

  i = m1;
  j = 1;
  r = (  i - 1 ) / ( m1 - 1 );
  s = (  j - 1 ) / ( m2 - 1 );
  x(i,j) = cubic_rs ( r, s, 1 );

  i = 1;
  j = m2;
  r = (  i - 1 ) / ( m1 - 1 );
  s = (  j - 1 ) / ( m2 - 1 );
  x(i,j) = cubic_rs ( r, s, 1 );

  i = m1;
  j = m2;
  r = (  i - 1 ) / ( m1 - 1 );
  s = (  j - 1 ) / ( m2 - 1 );
  x(i,j) = cubic_rs ( r, s, 1 );

  x = blend_ij_0d1 ( x, m1, m2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Values interpolated by BLEND_IJ_0D1:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : m1
    fprintf ( 1, '  %10f  %10f  %10f  %10f\n', x(i,1:m2) );
  end
%
%  Load data in the edges.
%
  j = 1;
  for i = 1 : m1
    r = (  i - 1 ) / ( m1 - 1 );
    s = (  j - 1 ) / ( m2 - 1 );
    x(i,j) = cubic_rs ( r, s, 1 );
  end

  j = m2;
  for i = 1 : m1
    r = (  i - 1 ) / ( m1 - 1 );
    s = (  j - 1 ) / ( m2 - 1 );
    x(i,j) = cubic_rs ( r, s, 1 );
  end

  i = 1;
  for j = 2 : m2 - 1
    r = (  i - 1 ) / ( m1 - 1 );
    s = (  j - 1 ) / ( m2 - 1 );
    x(i,j) = cubic_rs ( r, s, 1 );
  end

  i = m1;
  for j = 2 : m2 - 1
    r = (  i - 1 ) / ( m1 - 1 );
    s = (  j - 1 ) / ( m2 - 1 );
    x(i,j) = cubic_rs ( r, s, 1 );
  end

  x = blend_ij_1d1 ( x, m1, m2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Values interpolated by BLEND_IJ_1D1:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : m1
    fprintf ( 1, '  %10f  %10f  %10f  %10f\n', x(i,1:m2) );
  end
%
%  Compare with BLEND_RS_1D1
%
  for i = 1 : m1
    r = (  i - 1 ) / ( m1 - 1 );
    for j = 1 : m2
      s = (  j - 1 ) / ( m2 - 1 );
      x(i,j) = blend_rs_1dn ( r, s, 1, @cubic_rs );
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Data blended by BLEND_RS_1DN:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : m1
    fprintf ( 1, '  %10f  %10f  %10f  %10f\n', x(i,1:m2) );
  end
%
%  Load all data.
%
  for i = 1 : m1
    for j = 1 : m2
      r = (  i - 1 ) / ( m1 - 1 );
      s = (  j - 1 ) / ( m2 - 1 );
      x(i,j) = cubic_rs ( r, s, 1 );
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Exact data:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : m1
    fprintf ( 1, '  %10f  %10f  %10f  %10f\n', x(i,1:m2) );
  end

  return
end


function val=calcRMSE(xTrue,xEst,is3D)
%%CALCRMSE Compute the scalar root-mean-squared error (RMSE) of estimates
%          compared to true values.
%
%INPUTS: xTrue The truth data. This is either an xDimXNumSamples matrix or
%              an xDimXNXnumSamples matrix. The latter formulation is
%              useful when the MSE over multiple Monte Carlo runs of an
%              entire length-N track is desired. In the first formulation,
%              we take N=1. Alternatively, if the same true value is used
%              for all numSamples, then xTrue can just be an xDimXN matrix.
%              Alternatively, if xTrue is the same for all numSamples and
%              all N, then just an xDimX1 matrix can be passed. N and
%              numSamples are inferred from xEst.
%         xEst An xDimXnumSamples set of estimates or an xDimXNXnumSamples
%              set of estimates (if values at N times are desired).
%         is3D An optional indicating that xEst is 3D. This is only used if
%              xEst is a matrix. In such an instance, there is an ambiguity
%              whether xEst is truly 2D, so N=1, or whether numSamples=1
%              and xEst is 3D with the third dimension being 1. If this
%              parameter is omitted or an empty matrix is passed, it is
%              assumed that N=1.
%
%OUTPUTS: val A 1XN vector of scalar RMSE values.
%
%The RMSE is discussed with relation to alternative error measured in [1]
%and is given in Equation 1 of [1].
%
%EXAMPLE:
%Here, we show that the RMSE of samples of a Gaussian random variable is
%related to the square root of the trace of a covariance matrix.
% R=[28,   4, 10;
%     4,  22, 16;
%     10, 16, 16];%The covariance matrix.
% xTrue=[10;-20;30];
% numRuns=100000;
% xEst=GaussianD.rand(numRuns,xTrue,R);
% rootTrace=sqrt(trace(R))
% val=calcRMSE(xTrue,xEst)
%One will see that the root-trace and the sample RMSE are close.
%
%REFERENCES:
%[1] X. R. Li and Z. Zhao, "Measures of performance for evaluation of
%    estimators and filters," in Proceedings of SPIE: Conference on Signal
%    and Data processing of Small Targets, vol. 4473, San Diego, CA, 29
%    Jul. 2001, pp. 530-541.
%
%February 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(is3D))
    is3D=false;
end

xDim=size(xEst,1);
if(ismatrix(xEst)&&is3D==false)
    N=1;
    numSamples=size(xEst,2);
    xEst=reshape(xEst,[xDim,1,numSamples]);

    if(size(xTrue,2)==1)
        %If the true values are the same for all samples.
        xTrue=repmat(xTrue,[1,1,numSamples]);
    else
        xTrue=reshape(xTrue,[xDim,1,numSamples]);
    end
else
    N=size(xEst,2);
    numSamples=size(xEst,3);

    if(ismatrix(xTrue))
        if(size(xTrue,2)==1)
            %If the true values are the same for all samples and for all N.
            xTrue=repmat(xTrue,[1,N,numSamples]);
        else
            %If the true values are the same for all samples.
            xTrue=repmat(xTrue,[1,1,numSamples]);
        end
    end
end

val=zeros(1,N);
for k=1:N
    val(k)=sqrt(sum(sum((xEst(:,k,:)-xTrue(:,k,:)).^2,3)/numSamples));
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 [Spos,Sneg,vpos,vneg] = strengths_und_sign(W)
%STRENGTHS_UND_SIGN        Strength and weight
%
%   [Spos Sneg] = strengths_und_sign(W);
%   [Spos Sneg vpos vneg] = strengths_und_sign(W);
%
%   Node strength is the sum of weights of links connected to the node.
%
%   Inputs:     W,              undirected connection matrix with positive
%                               and negative weights
%
%   Output:     Spos/Sneg,      nodal strength of positive/negative weights
%               vpos/vneg,      total positive/negative weight
%
%
%   2011, Mika Rubinov, UNSW

%   Modification History:
%   Mar 2011: Original

n = length(W);              %number of nodes
W(1:n+1:end) = 0;           %clear diagonal
Spos = sum( W.*(W>0));      %positive strengths
Sneg = sum(-W.*(W<0));      %negative strengths

if nargout>2
    vpos = sum(Spos);       %positive weight
    vneg = sum(Sneg);       %negative weight
end


function [ xstar, seed ] = rk1_tv_step ( x, t, h, q, fv, gv, seed )

%*****************************************************************************80
%
%% RK1_TV_STEP takes one step of a stochastic Runge Kutta scheme.
%
%  Discussion:
%
%    The Runge-Kutta scheme is first-order, and suitable for time-varying
%    systems.
%
%    d/dx X(t,xsi) = F ( X(t,xsi), t ) + G ( X(t,xsi), t ) * w(t,xsi)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jeremy Kasdin,
%    Runge-Kutta algorithm for the numerical integration of
%    stochastic differential equations,
%    Journal of Guidance, Control, and Dynamics,
%    Volume 18, Number 1, January-February 1995, pages 114-120.
%
%    Jeremy Kasdin,
%    Discrete Simulation of Colored Noise and Stochastic Processes
%    and 1/f^a Power Law Noise Generation,
%    Proceedings of the IEEE,
%    Volume 83, Number 5, 1995, pages 802-827.
%
%  Parameters:
%
%    Input, real X, the value at the current time.
%
%    Input, real T, the current time.
%
%    Input, real H, the time step.
%
%    Input, real Q, the spectral density of the input white noise.
%
%    Input, external real FV, the name of the deterministic
%    right hand side function.
%
%    Input, external real GV, the name of the stochastic
%    right hand side function.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real XSTAR, the value at time T+H.
%
  a21 = 1.0;

  q1 = 1.0;

  t1 = t;
  x1 = x;
  [ n1, seed ] = r8_normal_01 ( seed );
  w1 = n1 * sqrt ( q1 * q / h );
  k1 = h * fv ( t1, x1 ) + h * gv ( t1, x1 ) * w1;

  xstar = x1 + a21 * k1;

  return
end


function varargout = qr(f, ignored)
%QR Orthogonal-triangular decomposition of a SEPARABLEAPPROX.
%
% [Q, R] = QR( F ), where F is a separableApprox, produces an unitary column
% quasimatrix Q and a upper-triangular row quasimatrix R so that F = Q * R. This
% is computed by a continuous analogue of QR.
%
% [Q, R] = QR( F, 0 ) is the same as QR( F ).
%
% [Q, R, E] = QR( F ) and [Q, R, E] = QR( F, 'vector') produces a vector E that
% stores the pivoting locations.
%
% For more information about this decomposition:
% A. Townsend and L. N. Trefethen, Continuous analogues of matrix
% factorizations, Proc. Royal Soc. A., 2015.
%
% See also LU, and CHOL.

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

% Empty check:
if ( isempty( f ) )
    varargout = cell(1, nargout);
    return
end

% As always start with the CDR decomposition:
[C, D, R] = cdr( f );

% Balance out the scaling, becareful about signs:
sgns = sign( diag( D ) );
C = C * diag(sgns) * sqrt( abs(D) );   % Put the signs into Q
R = R * sqrt( abs(D) );

% QR of the column
[Q, RC] = qr( C );

% Form R:
R = RC * R.';   % still an upper-triangular quasimatrix! :)

% Output to user:
if ( nargout <= 1 )
    varargout = { Q };
elseif ( nargout == 2 )
    varargout = { Q, R };
elseif ( nargout == 3 )
    E = f.pivotLocations;   % complete pivot Locations from GE.
    varargout = { Q, R, E( :, 1 ) };  % Only pass the ones in x.
end

end


function c = tapas_logrt_linear_binary_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the linear log-reaction time response model according to as
% developed with Louise Marshall and Sven Bestmann
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The Gaussian noise observation model assumes that responses have a Gaussian distribution around
% the inferred mean of the relevant state. The only parameter of the model is the noise variance
% (NOT standard deviation) zeta.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2016 Christoph Mathys, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'Linear log-reaction time for binary models';

% Sufficient statistics of Gaussian parameter priors
%
% Beta_0
c.be0mu = log(500);
c.be0sa = 4;

% Beta_1
c.be1mu = 0;
c.be1sa = 4;

% Beta_2
c.be2mu = 0;
c.be2sa = 4;

% Beta_3
c.be3mu = 0;
c.be3sa = 4;

% Beta_4
c.be4mu = 0;
c.be4sa = 4;

% Zeta
c.logzemu = log(log(20));
c.logzesa = log(2);

% Gather prior settings in vectors
c.priormus = [
    c.be0mu,...
    c.be1mu,...
    c.be2mu,...
    c.be3mu,...
    c.be4mu,...
    c.logzemu,...
         ];

c.priorsas = [
    c.be0sa,...
    c.be1sa,...
    c.be2sa,...
    c.be3sa,...
    c.be4sa,...
    c.logzesa,...
         ];

% Model filehandle
c.obs_fun = @tapas_logrt_linear_binary;

% Handle to function that transforms observation parameters to their native space
% from the space they are estimated in
c.transp_obs_fun = @tapas_logrt_linear_binary_transp;

return;


function H = per(sequence,isplot)
%
% 'per' estimate the hurst parameter of a given sequence with periodogram
%     method.
%
% Inputs:
%     sequence: the input sequence for estimate
%     isplot: whether display the plot. without a plot if isplot equal to 0
% Outputs:
%     H: the estimated hurst coeffeient of the input sequence

%  Author: Chu Chen
%  Version 1.0,  03/10/2008
%  chen-chu@163.com
%

if nargin == 1
    isplot = 0;
end

n = length(sequence);
Xk = fft(sequence);
P_origin = abs(Xk).^2/(2*pi*n);
P = P_origin(1:floor(n/2)+1);

x = log10((pi/n)*[2:floor(0.5*n)]);
y = log10(P(2:floor(0.5*n)));

% Use the lowest 20% part of periodogram to estimate the similarity.
X = x(1:floor(length(x)/5));
Y = y(1:floor(length(y)/5));
p1 = polyfit(X,Y,1);
Yfit = polyval(p1,X);
H = (1-(Yfit(end)-Yfit(1))/(X(end)-X(1)))/2;

if isplot ~= 0
    figure,clf,hold on;
    plot(x,y,'b.');
    plot(X,Yfit,'r-','LineWidth',3);
    xlabel('log10(Frequency)'),ylabel('log10(Periodogram)'),title('Periodogram Method');
end


function [m2] = ft22m2(ft2)
% Convert area from square feet to square meters.
% Chad A. Greene 2012
m2 = ft2*0.09290304;


function halton_test11 ( )

%*****************************************************************************80
%
%% TEST11 tests U2_TO_SPHERE_UNIT_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2009
%
%  Author:
%
%    John Burkardt
%
  dim_num2 = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST11\n' );
  fprintf ( 1, '  For the unit sphere in 3 dimensions:\n' );
  fprintf ( 1, '  U2_TO_SPHERE_UNIT_3D samples;\n' );

  dim_num = 2;
  halton_dim_num_set ( dim_num );
  n = 5;
  step = 123456789;
  halton_step_set ( step );
  seed(1:dim_num) = 0;
  halton_seed_set ( seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  DIM_NUM = %12d\n', dim_num );
  fprintf ( 1, '  N =    %12d\n', n );
  fprintf ( 1, '  STEP = %12d\n', step );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  A few sample values:\n' );
  fprintf ( 1, '\n' );

  for j = 1 : n
    u = halton ( );
    x = u2_to_sphere_unit_3d ( u );
    fprintf ( 1, '  ' );
    for i = 1 : dim_num2
      fprintf ( 1, '  %6f', x(i) );
    end
    fprintf ( 1, '\n' );
  end

  n = 1000;
  step = 0;
  halton_step_set ( step );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  N =    %12d\n', n );
  fprintf ( 1, '  STEP = %12d\n', step );

  average(1:dim_num2) = 0.0;

  for i = 1 : n
    u = halton ( );
    x = u2_to_sphere_unit_3d ( u );
    average(1:dim_num2) = average(1:dim_num2) + x(1:dim_num2);
  end

  average(1:dim_num2) = average(1:dim_num2) / n;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Average the points, which should get a value\n' );
  fprintf ( 1, '  close to zero, and closer as N increases.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Average:        ' );
  for i = 1 : dim_num2
    fprintf ( 1, '  %8e', average(i) );
  end
  fprintf ( 1, '\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now choose a random direction, sample the same\n' );
  fprintf ( 1, '  number of points, and compute the dot product with\n' );
  fprintf ( 1, '  the direction.\n' );
  fprintf ( 1, '  Take the absolute value of each dot product\n' );
  fprintf ( 1, '  and sum and average.\n' );

  for j2 = 1 : 5

    seed_val = get_seed ( );
    step = seed_val + 111 * j2;
    halton_step_set ( step );

    u = halton ( );
    v = u2_to_sphere_unit_3d ( u );

    step = 0;
    halton_step_set ( step );

    dot_average = 0.0;

    for j = 1 : n
      u = halton ( );
      x = u2_to_sphere_unit_3d ( u );
      dot_average = dot_average + abs ( x(1:dim_num2) * v(1:dim_num2)' );
    end

    dot_average = dot_average / n;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Random V:         ' );
    for j = 1 : dim_num2
      fprintf ( 1, '  %8f', v(j) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Average |(XdotV)| %f\n', dot_average );

  end

  return
end