function geometry_test174 ( )

%*****************************************************************************80
%
%% TEST174 tests RADEC_TO_XYZ, XYZ_TO_RADEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  ntest = 6;

  ptest = [ ...
     1.0,  0.0,  0.0; ...
     0.0,  1.0,  0.0; ...
     0.0,  0.0,  1.0; ...
     1.0,  1.0,  1.0; ...
     5.0, -2.0, -1.0; ...
    -2.0, -2.0, -2.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST174\n' );
  fprintf ( 1, '  RADEC_TO_XYZ converts XYZ to RADEC coordinates.\n' );
  fprintf ( 1, '  XYZ_TO_RADEC converts RADEC to XYZ coordinates.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '          P1          RA     DEC           P2\n' );
  fprintf ( 1, '\n' );

  for i = 1 : ntest

    p1(1:3,1) = ptest(1:3,i);

    [ ra, dec ] = xyz_to_radec ( p1 );
    p2 = radec_to_xyz ( ra, dec );

    fprintf ( 1, '  %10f  %10f  %10f  %10f  %10f  %10f  %10f  %10f\n', ...
      p1(1:3,1), ra, dec, p2(1:3,1) );

  end

  return
end


function r8vm_print ( m, n, a, title )

%*****************************************************************************80
%
%% R8VM_PRINT prints a R8VM matrix.
%
%  Discussion:
%
%    The R8VM storage format is used for an M by N Vandermonde matrix.
%    An M by N Vandermonde matrix is defined by the values in its second
%    row, which will be written here as X(1:N).  The matrix has a first
%    row of 1's, a second row equal to X(1:N), a third row whose entries
%    are the squares of the X values, up to the M-th row whose entries
%    are the (M-1)th powers of the X values.  The matrix can be stored
%    compactly by listing just the values X(1:N).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of the matrix.
%
%    Input, real A(N), the R8VM matrix.
%
%    Input, string TITLE, a title to be printed.
%
  r8vm_print_some ( m, n, a, 1, 1, n, n, title );

  return
end


function [p,w] = nrbeval(nurbs,tt)
%
% Function Name:
%
%   nrbeval - Evaluate a NURBS at parameteric points
%
% Calling Sequence:
%
%   [p,w] = nrbeval(crv,ut)
%   [p,w] = nrbeval(srf,{ut,vt})
%
% Parameters:
%
%   crv		: NURBS curve, see nrbmak.
%
%   srf		: NURBS surface, see nrbmak.
%
%   ut		: Parametric evaluation points along U direction.
%
%   vt		: Parametric evaluation points along V direction.
%
%   p		: Evaluated points on the NURBS curve or surface as cartesian
% 		coordinates (x,y,z). If w is included on the lhs argument list
% 		the points are returned as homogeneous coordinates (wx,wy,wz).
%
%   w		: Weights of the homogeneous coordinates of the evaluated
% 		points. Note inclusion of this argument changes the type
% 		of coordinates returned in p (see above).
%
% Description:
%
%   Evaluation of NURBS curves or surfaces at parametric points along the
%   U and V directions. Either homogeneous coordinates are returned if the
%   weights are requested in the lhs arguments, or as cartesian coordinates.
%   This function utilises the 'C' interface bspeval.
%
% Examples:
%
%   Evaluate the NURBS circle at twenty points from 0.0 to 1.0
%
%   nrb = nrbcirc;
%   ut = linspace(0.0,1.0,20);
%   p = nrbeval(nrb,ut);
%
% See:
%
%     bspeval
%

%  D.M. Spink
%  Copyright (c) 2000.

if nargin < 2
  error('Not enough input arguments');
end

foption = 1;    % output format 3D cartesian coordinates
if nargout == 2
  foption = 0;  % output format 4D homogenous coordinates
end

if ~isstruct(nurbs)
  error('NURBS representation is not structure!');
end

if ~strcmp(nurbs.form,'B-NURBS')
  error('Not a recognised NURBS representation');
end

if iscell(nurbs.knots)
  % NURBS structure represents a surface

  num1 = nurbs.number(1);
  num2 = nurbs.number(2);
  degree = nurbs.order-1;

  if iscell(tt)
    % Evaluate over a [u,v] grid
    % tt{1} represents the u direction
    % tt{2} represents the v direction

    nt1 = length(tt{1});
    nt2 = length(tt{2});

    % Evaluate along the v direction
    val = reshape(nurbs.coefs,4*num1,num2);
    val = bspeval(degree(2),val,nurbs.knots{2},tt{2});
    val = reshape(val,[4 num1 nt2]);

    % Evaluate along the u direction
    val = permute(val,[1 3 2]);
    val = reshape(val,4*nt2,num1);
    val = bspeval(degree(1),val,nurbs.knots{1},tt{1});
    val = reshape(val,[4 nt2 nt1]);
    val = permute(val,[1 3 2]);

    w = val(4,:,:);
    p = val(1:3,:,:);
    if foption
      p = p./repmat(w,[3 1 1]);
    end

  else

    % Evaluate at scattered points
    % tt(1,:) represents the u direction
    % tt(2,:) represents the v direction

    nt = size(tt,2);

    val = reshape(nurbs.coefs,4*num1,num2);
    val = bspeval(degree(2),val,nurbs.knots{2},tt(2,:));
    val = reshape(val,[4 num1 nt]);

    % evaluate along the u direction
    pnts = zeros(4,nt);
    for v = 1:nt
      coefs = squeeze(val(:,:,v));
      pnts(:,v) = bspeval(degree(1),coefs,nurbs.knots{1},tt(1,v));
    end

    w = pnts(4,:);
    p = pnts(1:3,:);
    if foption
       p = p./w;
    end

  end

else

  % NURBS structure represents a curve
  %  tt represent a vector of parametric points in the u direction

  val = bspeval(nurbs.order-1,nurbs.coefs,nurbs.knots,tt);

  w = val(4,:);
  p = val(1:3,:);
  if foption
    p = p./repmat(w,3,1);
  end

end


function [maskOut]=kGaussian_color_EM(imageFile,k)
%% this function uses EM algorithm and gaussian distribution function to do
% image segmentation on the color space
% try:
%   img=imread('onion.png');
%   [maskOut]=kGaussian_color_EM(img,9); figure;  imshow(maskOut)
% written by Rongwen Lu on 12/11/2011; email: rongwen@uab.edu
img=double(imageFile);

[M,N,P]=size(img);
n=M*N;
imgR=img(:,:,1);
imgG=img(:,:,2);
imgB=img(:,:,3);
[cy,cx]=ind2sub([M,N],1:n);

%normalize each vector; delete it if normalization is not needed; weights
%are also assigned here;
% imgR=mat2gray(imgR);
% imgG=mat2gray(imgG);
% imgB=mat2gray(imgB);
% cy=mat2gray(cy);
% cx=mat2gray(cx);
imgR=imgR/255;
imgG=imgG/255;
imgB=imgB/255;
cy=cy/M;
cx=cx/N;

% %% Gaussian filter
% w=fspecial('gaussian',[5,5],4);
% imgR=imfilter(imgR,w);
% imgG=imfilter(imgG,w);
% imgB=imfilter(imgB,w);

%% assign vectors into the matrix raw
raw=zeros(n,3);
raw(:,1)=imgR(:);
raw(:,2)=imgG(:);
raw(:,3)=imgB(:);

% raw=zeros(n,5);
% raw(:,1)=cy.';
% raw(:,2)=cx.';
% raw(:,3)=imgR(:);
% raw(:,4)=imgG(:);
% raw(:,5)=imgB(:);

%% get assignment matrix p, which is also memebership probability here; u
%% is vector of the estimated means of Gaussian function, v is the vector ot the estimated SD
[p,u,v]=em(raw,k);

imgRe=zeros(n,3);
kColor=jet(k);
kColor=u(:,1:3);
imgRe=p*kColor;

maskOut=zeros(M,N,3);
for ii=1:3
    maskOut(:,:,ii)=reshape(imgRe(:,ii),[M,N]);
end

% figure; imshow((maskOut))
% title('based on k Gaussian by EM algorithm on color space')

 function [p,u,v]=em(raw,k)
 % this function uses Expectation Maximization method to estimate k
 % Gaussian distribution functions. There are 2 input arguments. Raw's size
 % is [n,dim], where n is the NO of data and dim is the dimention of data.
 % k means using k Gaussin functions to do image segmentation.The output is
 % p, which is assignment matrix.its [ii,jj]th element means the
 % probability that Xii is generated by jjth Gaussian function.

%% the input of following codes are raw,and K

%% initialize centroid matrix u; u has k raws. NO of column is the same as
%% raw
[n,dim]=size(raw);
u=raw(randint(k,1,[1,n]),:);
%u=[0,0;40,40;-40,-40];

%% initialize standard diviation v, v has k raws, one column.
v=zeros(k,1);
for ii=1:k
    raw_tmp=raw(ii:k:end,1);
    v(ii,:)=std(raw_tmp);
end
%v=.5*[4,4,4]';
%% initialize weight w
w=ones(k,1)/k;

%% initialize membership probability matrix (assignment matrix) p, which is p(k|x)
p=zeros(n,k);

%% do interation to get best r
u0=u*0;
v0=0*v;
w0=w*0;
energy=sum(sum((u-u0).^2))+sum(sum((v-v0).^2))+(sum((w-w0).^2));
iteration=1;
x_u=zeros(size(raw));
while energy>10^(-6)

    %% calculate membership probability, which is also assignment matrix
    for jj=1:k
        for ss=1:dim
            x_u(:,ss)=raw(:,ss)-u(jj,ss)*ones(n,1);
        end
        x_u=x_u.*x_u;
        p(:,jj)=power(sqrt(2*pi)*v(jj),-1*dim)*exp((-1/2)*sum(x_u,2)./(v(jj).^2));
        p(:,jj)=p(:,jj)*w(jj);

    end
    %% normalize p on the x dimention
    pSum=sum(p,2);
    for jj=1:k
        p(:,jj)=p(:,jj)./pSum;
    end

    %% normlaize p on the y dimention, yielding pNorm
    pSum2=sum(p,1);
    pNorm=p*0;
    for jj=1:k
        pNorm(:,jj)=p(:,jj)/pSum2(jj);
    end

    %% save current u, v, and w as u0, v0 and w0
    u0=u;v0=v;w0=w;

    %%update u
    u=(pNorm.')*raw;

    %% update v
    for jj=1:k
        for ss=1:dim
            x_u(:,ss)=raw(:,ss)-u(jj,ss)*ones(n,1);
        end
        x_u=x_u.*x_u;
        x_uSum=sum(x_u,2);
        v(jj)=sqrt(1/dim*(pNorm(:,jj).')*x_uSum);
    end

    %% update w
    w=(sum(p)/n).';

    %% update and display iteration and energy

    %disp(sprintf(['iteration=',num2str(iteration),'; energy=',num2str(energy,'%g')]))
    iteration=iteration+1;
    energy=sum(sum((u-u0).^2))+sum(sum((v-v0).^2))+(sum((w-w0).^2));

end


function [ xtab, weight ] = laguerre_compute ( norder, alpha )

%*****************************************************************************80
%
%% LAGUERRE_COMPUTE computes a Gauss-Laguerre quadrature rule.
%
%  Discussion:
%
%    In the simplest case, ALPHA is 0, and we are approximating the
%    integral from 0 to +oo of EXP(-X) * F(X).  When this is so,
%    it is easy to modify the rule to approximate the integral from
%    A to +oo as well.
%
%    If ALPHA is nonzero, then there is no simple way to extend the
%    rule to approximate the integral from A to +oo.  The simplest
%    procedures would be to approximate the integral from 0 to A.
%
%    The integration interval is [ A, +oo ) or [ 0, +oo ).
%
%    The weight function w(x) = EXP ( - X ) or EXP ( - X ) * X^ALPHA.
%
%
%    If the integral to approximate is:
%
%        Integral ( A <= X < +oo ) EXP ( - X ) * F(X) dX
%      or
%        Integral ( 0 <= X < +oo ) EXP ( - X ) * X^ALPHA * F(X) dX
%
%    then the quadrature rule is:
%
%      EXP ( - A ) * Sum ( 1 <= I <= ORDER ) WEIGHT(I) * F ( A+XTAB(I) )
%    or
%      sum ( 1 <= I <= ORDER ) WEIGHT(I) * F ( XTAB(I) )
%
%
%    If the integral to approximate is:
%
%        Integral ( A <= X < +oo ) F(X) dX
%      or
%        Integral ( 0 <= X < +oo ) X^ALPHA * F(X) dX
%
%    then the quadrature rule is:
%
%      EXP ( - A ) * sum ( 1 <= I <= ORDER )
%        WEIGHT(I) * EXP(A+XTAB(I)) * F ( A+XTAB(I) )
%    or
%      sum ( 1 <= I <= ORDER ) WEIGHT(I) * EXP(XTAB(I)) * F ( XTAB(I) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2005
%
%  Author:
%
%    Original FORTRAN77 version by Arthur Stroud, Don Secrest.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, integer NORDER, the order of the quadrature rule to be computed.
%    NORDER must be at least 1.
%
%    Input, real ALPHA, the exponent of the X factor.
%    Set ALPHA = 0.0 for the simplest rule.
%    ALPHA must be nonnegative.
%
%    Output, real XTAB(NORDER), the Gauss-Laguerre abscissas.
%
%    Output, real WEIGHT(NORDER), the Gauss-Laguerre weights.
%

%
%  Set the recursion coefficients.
%
  for i = 1 : norder
    b(i) = ( alpha + 2 * i - 1 );
  end

  for i = 1 : norder
    c(i) = ( i - 1 ) * ( alpha + i - 1 );
  end

  cc = gamma ( alpha + 1.0 ) * prod ( c(2:norder) );

  for i = 1 : norder
%
%  Compute an estimate for the root.
%
    if ( i == 1 )

      x = ( 1.0 + alpha ) * ( 3.0 + 0.92 * alpha ) ...
        / ( 1.0 + 2.4 * norder + 1.8 * alpha );

    elseif ( i == 2 )

      x = x + ( 15.0 + 6.25 * alpha ) ...
        / ( 1.0 + 0.9 * alpha + 2.5 * norder );

    else

      r1 = ( 1.0 + 2.55 * ( i - 2 ) ) / ( 1.9 * ( i - 2 ) );

      r2 = 1.26 * ( i - 2 ) * alpha / ( 1.0 + 3.5 * ( i - 2 ) );

      ratio = ( r1 + r2 ) / ( 1.0 + 0.3 * alpha );

      x = x + ratio * ( x - xtab(i-2) );

    end
%
%  Use iteration to find the root.
%
    [ x, dp2, p1 ] = laguerre_root ( x, norder, alpha, b, c );
%
%  Set the abscissa and weight.
%
    xtab(i) = x;
    weight(i) = ( cc / dp2 ) / p1;

  end

  return
end


function [N]=gold_n(wl)

% the function calculates the index of refraction of gold (complex value), in the range of wavelength
% between 188nm and 1942nm.
% the input may be a scalar or vector, but it have to be an integer and in the right range.
% the values were taken from the paper:
%       P. B. Johnson, and R. W. Christy, "Optical Constants of the Noble Metals," Physical Review B 6, 4370 (1972).
% (the paper is available here: http://prb.aps.org/abstract/PRB/v6/i12/p4370_1 )
% the values for wavelength calculated with linear interpolation.
%
% Moshe Lindner, August 2010 (C).

if any((max(wl)>1942) || (min(wl)<188))
    error('Wavelength is out of range! (should be in the range of 188nm - 1942nm)')
    return
elseif any(wl~=ceil(wl))
    error('Wavelength must be an integer!')
    return
end

% n = real part
n=[0.92 0.56 0.43 0.35 0.27 0.22 0.17 0.16 0.14 0.13 0.14 0.21 0.29 0.43 0.62 1.04 1.31 1.39 1.45 1.46 1.47 1.46 1.48 1.5 1.48 1.48 1.54 1.53 1.53 1.49 1.47 1.43 1.38 1.35 1.33 1.33 1.32 1.32 1.3 1.31 1.3 1.3 1.3 1.3 1.33 1.33 1.34 1.32 1.28]';
n=n(end:-1:1);
% k = imaginary part
k=[13.78 11.21 9.519 8.145 7.15 6.35 5.663 5.083 4.542 4.103 3.697 3.272 2.863 2.455 2.081 1.833 1.849 1.914 1.948 1.958 1.952 1.933 1.895 1.866 1.871 1.883 1.898 1.893 1.889 1.878 1.869 1.847 1.803 1.749 1.688 1.631 1.577 1.536 1.497 1.46 1.427 1.387 1.35 1.304 1.277 1.251 1.226 1.203 1.188]';
k=k(end:-1:1);
% ev = photon energy in electronvolts
ev=[0.64 0.77 0.89 1.02 1.14 1.26 1.39 1.51 1.64 1.76 1.88 2.01 2.13 2.26 2.38 2.50 2.63 2.75 2.88 3 3.12 3.25 3.37 3.5 3.62 3.74 3.87 3.99 4.12 4.24 4.36 4.49 4.61 4.74 4.86 4.98 5.11 5.23 5.36 5.48 5.6 5.73 5.85 5.98 6.1 6.22 6.35 6.47 6.6]';
ev=ev(end:-1:1);
h=6.62606896e-34; % [J*sec] (Plank const.)
c=3e8; %[m/sec]  (speed of light)
q=1.6e-19; % [C] (electron charge)
lambda=1e9*h*c./(ev.*q);% wavelength in nanometer

lam=188:1942;
n1=interp1(lambda,n,lam,'linear');
k1=interp1(lambda,k,lam,'linear');

for q=1:length(wl)
    N(q)=n1(lam==wl(q)) + i*k1(lam==wl(q));
end


function B = bernstein_eval(i,n,u)
  % BERNSTERIN_EVAL evaluate a Bernstein polynomial. B_i,n(u)
  %
  % B = bernstein_eval(i,n,u)
  %
  % Inputs:
  %  i  #i scalar index of polynomial
  %  n  scalar order of polynomial
  %  u  #u evaluation points
  % Outputs:
  %  B  #u by #i values
  %
  u = reshape(u,numel(u),1);
  i = reshape(i,1,numel(i));
  nck = factorial(n)./(factorial(i).*factorial(n-(i)));
  B = nck .*  u.^i .* (1-u).^(n-i);
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% Problem 7- Frequency Response and Impulse Response of an ideal bandpass
% filter

td=.05;
B1=10 ;
B2=20;
w1=-30:-B2;
H1=zeros(size(w1));
w2=-B2:-B1;
H2=exp(-j*w2*td);
w3=-B1:B1;
H3=zeros(size(w3));
w4=B1:B2;
H4= exp(-j*w4*td);
w5=B2:30;
H5=zeros(size(w5));
w=[w1 w2 w3 w4 w5];
H=[H1 H2 H3 H4 H5];
plot(w,abs(H))
ylim([-.2 1.2]);
legend(' |H(\Omega) | ')

figure
plot(w,angle(H));
ylim([-1.1 1.1]);
legend('\angle H(\Omega)')

figure
syms t w
H1=exp(-j*w*td)*(heaviside(w+B2)-heaviside(w+B1));
H2=exp(-j*w*td)*(heaviside(w-B1)-heaviside(w-B2));
h1=ifourier(H1,t);
h2=ifourier(H2,t);
h=h1+h2;
ezplot(h, [-3 3]);
ylim([-4 4]);
legend('Impulse response h(t) ')


function [Vc,R]=circlefit(V)

% function [Vc,R]=circlefit(V)
% ------------------------------------------------------------------------
% This function fits a circle to 2 or more points. The input is an array
% containing the points (vertices) V in 2D or 3D. Although 3D data is
% supported the Z-coordinates should all be equal such that the fitting
% problem remains 2D.
% The output consists of the circle centre Vc and radius R.
%
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
% 2019/11/21 Added to GIBBON
% ------------------------------------------------------------------------

%% Parse input

%Check if empty or not numeric
if isempty(V)
    error('Empty input')
elseif ~isnumeric(V)
    error('Input is not numeric');
end

%Force real
if ~isreal(V)
    V=real(V);
    warning('Complex data encountered. Complex parts ignored');
end

%Force double
if ~isa(V,'double')
    V=double(V); %Attempt conversion to double
end

%Check size
sizV=size(V);
if sizV(1)==1
    error('Only 1 point provided')
end

%Force 3D
if sizV(2)==2
    V(:,3)=0;
end

%% Fit cirlce
Vm=mean(V,1); %Get mean of point set
VD=V-Vm(ones(sizV(1),1),:); % shift points on mean
D=sqrt(sum(VD.^2,2)); %Compute distance to centre

if sizV(1)==2 %If only two points provided
    Vc=Vm; %Cirlc centre set to mean
    R=mean(D); %Radius set to mean distance
else %Fit circle to more than 2 points
    % Scale data
    scaleFactor=max(max(D,eps));
    VD=VD./scaleFactor; D=D./scaleFactor;

    % Solve sytem
    VF=[2*VD(:,1) 2*VD(:,2) ones(sizV(1),1)]\D.^2;

    % Unscale, collect output
    Q=[Vm(1) Vm(2) 0]+[VF(1) VF(2) sqrt(VF(3)+hypot(VF(1),VF(2)).^2)]*scaleFactor;

    Vc=[Q(1) Q(2) Vm(3)]; %Cirle centre
    R=Q(3); %Radius
end

if sizV(2)==2
    Vc=Vc(:,[1 2]);
end

end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function value = p31_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P31_F evaluates the integrand for problem 31.
%
%  Dimension:
%
%    DIM_NUM arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integral Parameters:
%
%    There is a basis point Z associated with the integrand.
%    Z(1:DIM_NUM) defaults to ( 0.5, 0.5, ..., 0.5 ).
%    The user can set, get, or randomize this value by calling
%    P31_R8VEC.
%
%    The coefficient vector C (whose entries are usually positive)
%    controls the steepness and circularity of the pseudo-Gaussian.
%    C(1:DIM_NUM) defaults to 2.0.
%    The user can set, get, or randomize this value by calling
%    P31_R8VEC.
%
%  Integrand:
%
%    exp ( - sum ( c(1:dim_num) * abs ( x(1:dim_num) - z(1:dim_num) ) ) )
%
%  Exact Integral:
%
%    The integral is separable into
%
%      Int ( A(1) <= X(1) <= B(1) ) exp ( - C(1) * abs ( X(1) - Z(1) ) )
%        * Int ( A(2) <= X(2) <= B(2) ) exp ( - C(2) * abs ( X(2) - Z(2) ) )
%          * ...
%
%    Hence, the exact integral is computed as the product of
%    one dimensional integrals.  Each of these is easily computed
%    once the location of Z(I) with respect to A(I) and B(I) is
%    determined.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Alan Genz,
%    [Integral #5]
%    A Package for Testing Multiple Integration Subroutines,
%    in Numerical Integration: Recent Developments, Software
%    and Applications,
%    edited by Patrick Keast and Graeme Fairweather,
%    D. Reidel, 1987, pages 337-340,
%    LC: QA299.3.N38.
%
%    Kenneth Hanson,
%    Quasi-Monte Carlo: halftoning in high dimensions?
%    in Computatinal Imaging,
%    Edited by CA Bouman and RL Stevenson,
%    Proceedings SPIE,
%    Volume 5016, 2003, pages 161-172.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  c = [];
  c = p31_r8vec ( 'G', 'C', dim_num, c );

  z = [];
  z = p31_r8vec ( 'G', 'Z', dim_num, z );

  value(1:point_num) = 0.0;

  for point = 1 : point_num

    value(point) = exp ( - sum ( c(1:dim_num)' .* ...
      abs ( x(1:dim_num,point) - z(1:dim_num)' ) ) );

  end

  p31_i4 ( 'I', '#', point_num );

  return
end


%% Test Problems for LEVMAR Interface
% J.Currie 2012
clc
clear all

%% NLS2 [0.3578]
clc
%Function
i = (1:40)';
fun = @(x) x(1)*exp(-x(2)*i) + x(3);
%Fitting Data
ydata=[5.8728, 5.4948, 5.0081, 4.5929, 4.3574, 4.1198, 3.6843, 3.3642, 2.9742, 3.0237, 2.7002, 2.8781,...
       2.5144, 2.4432, 2.2894, 2.0938, 1.9265, 2.1271, 1.8387, 1.7791, 1.6686, 1.6232, 1.571, 1.6057,...
       1.3825, 1.5087, 1.3624, 1.4206, 1.2097, 1.3129, 1.131, 1.306, 1.2008, 1.3469, 1.1837, 1.2102,...
       0.96518, 1.2129, 1.2003, 1.0743];
% %Setup Options
opts = optiset('solver','levmar');
% %Build & Solve
x0=[1.0; 1.0; 1.0];
Opt = opti('fun',fun,'ydata',ydata,'ndec',3,'options',opts)
[x,fval,exitflag,info] = solve(Opt,x0)

%%
clc
grad = @(x) mklJac(fun,x);

opts = [];
opts.display = 2;
opts.maxiter = 50;
lb = [];[0;0.5;2];
ub = [];[10;10;10];

[x,f,e,i,ii] = levmar(fun,[],x0,ydata,lb,ub,[],[],[],[],opts)

%% NLS2 [0.3578]
clc
%Function
i = (1:40)';
fun = @(x) exp(-x(1)*i);
%Fitting Data
ydata=[5.8728, 5.4948, 5.0081, 4.5929, 4.3574, 4.1198, 3.6843, 3.3642, 2.9742, 3.0237, 2.7002, 2.8781,...
       2.5144, 2.4432, 2.2894, 2.0938, 1.9265, 2.1271, 1.8387, 1.7791, 1.6686, 1.6232, 1.571, 1.6057,...
       1.3825, 1.5087, 1.3624, 1.4206, 1.2097, 1.3129, 1.131, 1.306, 1.2008, 1.3469, 1.1837, 1.2102,...
       0.96518, 1.2129, 1.2003, 1.0743];
% %Setup Options
opts = optiset('solver','levmar');
% %Build & Solve
x0=[1.0];
Opt = opti('fun',fun,'ydata',ydata,'ndec',1,'options',opts)
[x,fval,exitflag,info] = solve(Opt,x0)

%%
grad = @(x) mklJac(fun,x);

opts = [];
opts.display = 2;
opts.maxiter = 50;
lb = [0];
ub = [10];

[x,f,e,i,ii] = nl2sol(fun,grad,x0,ydata,[],[],opts)

%%
clc
prob = nls_prob(20);
opts = optiset('solver','nl2sol','display','iter');
Opt = opti(prob,opts)

[x,f,e,i] = solve(Opt)


function res = SymPoly2Sym(F,Dexp,MaxCtr)
%SYMPOLY2SYM   Similar to poly2sym but can handle symbolic coefficients and
%indeterminate variables specified as equations
%
%   SymPoly2Sym('a*z^2 + b*z + c','z') = [a b c]
%   SymPoly2Sym('a*sin(x)^2 + b*sin(x) + c','sin(x)') = [a b c]

%%This function iteratively takes the derivative then evaluates at 0 to
%%determine the coefficients

if nargin<2,
    Dexp = 't'; %Default dependent variable
end
F = sym(subs(F,Dexp,'XX')); %Substitute independent variable 'XX' for dependent expression Dexp

if nargin<3,
    MaxCtr = 20; %Default maximum order
end

res = sym([]); %Initialize res as empty sym
ctr = 0; %Initialize counter
while F~=0,
    res(end+1) = subs(F,'XX',0); %Evaluate at XX=0
    F = diff(F,'XX',1); %Take derivative

    %%%%Check ctr as a defense against recursive loops
    ctr = ctr+1;
    if ctr>=MaxCtr,
        disp(['Error: Exceeded maximum number of terms (' num2str(MaxCtr) ')'])
        disp(['Inputs may be in invalid form (not a polynomial)'])
        disp('To increase maximum number, use SymPoly2Sym(F,DV,Max#)')
        res = [];
        return
    end
    %%%%%%%%

end
Fact = [1 cumprod(1:length(res)-1)]; %Create factorial vector
res = res./Fact; %Divide coefficients by the amount contributed by derivative operations
res = res(end:-1:1); %Reverse order to match poly2sym convention


function [ problem ] = barbara_problem( )
%sBARBARATEST Prepares the test s of learning dictionary from
% Barbara image
s.image = spx.data.standard_images.barbara_gray_512x512;
% Block size
s.blkSize = 8;
% Number of atoms in dictionary
s.D = 121;
% Signal dimension
s.N = s.blkSize * s.blkSize;
% Let us create all the patches from image
s.patches = im2col(s.image, [s.blkSize, s.blkSize], 'sliding');
% Select 10 % of patches as training patches
s.training_patches = s.patches(:, 1:10:end);
% Let us construct a 1D-DCT matrix of size 8x11.
dr = sqrt(s.D);
DCT=zeros(s.blkSize,dr);
for k=0:1:dr-1,
    is = 0:1:s.blkSize-1;
    % Create one column
    col=cos(is'*k*pi/dr);
    if k>0
        col=col-mean(col);
    end;
    DCT(:,k+1)=col/norm(col);
end;
% We create our initial dictionary as a 64x121 size
s.initial_dictionary = kron(DCT, DCT);
s.DCT = DCT;
% We return the setup
problem = s;
end


%MAIN
%
% This script is used for testing the continuous, finite horizon, LQR
% solver that I'm working on.
%
%
clear; clc;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                       User Set Parameters                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

nSim = 25;
testPerturbation = 0.1;  %Initial position error amplitude

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                       Draw the dynamical system                         %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

n = 13;
xLim = [-4,1.5];
yLim = [-4,1];
x = linspace(xLim(1),xLim(2),n);
y = linspace(yLim(1),yLim(2),n);

[xx,yy] = ndgrid(x,y);

[dxx, dyy] = dynamics(xx,yy,zeros(n,n));

figure(1); clf; hold on;
title('Dynamical System - Simulations');
quiver(xx,yy,dxx,dyy);
xlabel('x');
ylabel('y');

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                    Compute a reference trajectory                       %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

options = odeset();
options.Events = @(t,z)eventFunc(t,z,xLim,yLim);
tSpan = [0,-10];

control = @(t) -2*sin(0.4*t-0.1)+0.6*cos(0.9*t+0.1);

userFunc = @(t,z)rhs(t,z,control(t));
z0 = [0;0];

sol = ode45(userFunc,tSpan,z0,options);

nTime = 70;
tSol = linspace(sol.x(end),sol.x(1),nTime);
zSol = deval(sol,tSol);
xSol = zSol(1,:);
ySol = zSol(2,:);
uSol = control(tSol);
tSpan = [tSol(1),tSol(end)];

plot(xSol,ySol,'r-','LineWidth',3);
plot(0,0,'ro','MarkerSize',10,'LineWidth',2)
axis(1.2*[xLim,yLim]); axis equal;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                      Store the reference trajectory                     %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

%We will store each trajectory as a polynomial. For low-order fits, say
%less than 10th order, Matlab's polyval and polyfit are good. For
%higher-order fit's, it is best to use different method, based on
%barycentric interpolation. Google chebyfun.

nFit = 5;  %Order of polynomial fitting
xFit = polyfit(tSol,xSol,nFit);
yFit = polyfit(tSol,ySol,nFit);
uFit = polyfit(tSol,uSol,nFit);

figure(2); clf;
subplot(3,1,1); hold on;
plot(tSol,xSol,'k.','MarkerSize',15)
plot(tSol,polyval(xFit,tSol),'r-')
xlim(tSpan);
ylabel('x')
title('Polynomial approximation of trajectory')
subplot(3,1,2); hold on;
plot(tSol,ySol,'k.','MarkerSize',15)
plot(tSol,polyval(yFit,tSol),'r-')
xlim(tSpan);
ylabel('y')
subplot(3,1,3); hold on;
plot(tSol,uSol,'k.','MarkerSize',15)
plot(tSol,polyval(uFit,tSol),'r-')
xlim(tSpan);
ylabel('u')
xlabel('t')

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                    Compute LQR Gains along trajectory                   %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

%Function handle for getting linearized system dynamics
linSys = @(t)getLinTraj(t,xFit,yFit,uFit);

Q = eye(2);  % Running cost on state
R = 1;       % Running cost on input
F = eye(2);  % Terminal cost on state
tol = 1e-6;  % Accuracy of ricatti propagation

Soln = trajectoryLqr(tSol,linSys,Q,R,F,tol);

K = reshape([Soln.K],2,nTime);
kxFit = polyfit(tSol,K(1,:),nFit);
kyFit = polyfit(tSol,K(2,:),nFit);

figure(3); clf;
subplot(2,1,1); hold on;
plot(tSol,K(1,:),'k.','MarkerSize',15)
plot(tSol,polyval(kxFit,tSol),'r-')
xlim(tSpan);
ylabel('Kx')
title('Polynomial approximation of lqr gains')
subplot(2,1,2); hold on;
plot(tSol,K(2,:),'k.','MarkerSize',15)
plot(tSol,polyval(kyFit,tSol),'r-')
xlim(tSpan);
ylabel('Ky')
xlabel('t')

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                    Compare to infinite-horizon LQR gains                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

ssSoln(nTime).t = 0;
ssSoln(nTime).K = zeros(1,2);
ssSoln(nTime).S = zeros(2);
ssSoln(nTime).E = zeros(2,1);
for i=1:nTime
    tNow = tSol(i);
    [A,B] = linSys(tNow);
    [KK,SS,EE] = lqr(A,B,Q,R);
    ssSoln(i).t = 0;
    ssSoln(i).K = KK;
    ssSoln(i).S = SS;
    ssSoln(i).E = EE;
end

ssK = reshape([ssSoln.K],2,nTime);
figure(4); clf;
subplot(2,1,1); hold on;
plot(tSol,K(1,:),'k.','MarkerSize',15)
plot(tSol,ssK(1,:),'bo','MarkerSize',10)
legend('finite','infinite')
xlim(tSpan);
ylabel('Kx')
title('Compare finite vs infinite horizon gains')
subplot(2,1,2); hold on;
plot(tSol,K(2,:),'k.','MarkerSize',15)
plot(tSol,ssK(2,:),'bo','MarkerSize',10)
legend('finite','infinite')
xlim(tSpan);
ylabel('Ky')
xlabel('t')

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                     Lyapunov Stability Analysis                         %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% Let V(x) = x'Sx, where S is the cost-to-go matrix from LQR solution:

%%%% TODO %%%%

% Plan:
%
%   1) Represent the S matrix, which is time-varying, as a polynomial.
%
%   2) Compute the derivative of S wrt time.
%
%   3) Try the Lyapunov function:
%           V(x,t) = x'*S(t)*x     % x = perturbation from nominal
%           dV(x,t) = 2*x*f(x,t)*S(t)*dS(t)   %Chain Rule  (check math...)
%
%   4) Verify stability by checking signs of lyapunov function and
%      derivative?
%
%   5) Not sure if this whole approach is correct - check against papers...
%

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                         Run a bunch of simulations                      %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

controller = @(t,z)stabilizingController(t,z(1,:),z(2,:),xFit,yFit,uFit,kxFit,kyFit);
userFunc = @(t,z)rhs(t,z,controller(t,z));
zNom = [xSol(1);ySol(1)];

xSim = zeros(nTime,nSim);
ySim = zeros(nTime,nSim);
uSim = zeros(nTime,nSim);
failFlag = false(nSim,1);
for i=1:nSim
    try
        z0 = zNom + testPerturbation*randn(2,1);
        [~, yout] = ode45(userFunc,tSol,z0);
        xSim(:,i) = yout(:,1);
        ySim(:,i) = yout(:,2);
        uSim(:,i) = controller(tSol,yout');
    catch ME
        xSim(:,i) = z0(1);
        ySim(:,i) = z0(2);
        uSim(:,i) = 0;
        failFlag(i) = true;
    end
end

% Plot the simulated trajectories:
figure(1); hold on;  %Plot on top of dynamics vector field
for i=1:nSim
    plot(xSim(:,i),ySim(:,i),'k-','LineWidth',1);
end

%%%% Plot trajectories against time:
figure(5); clf;

for i=1:nSim
    if ~failFlag(i)
        subplot(3,1,1); hold on;
        plot(tSol,xSim(:,i),'k-','LineWidth',1)
        subplot(3,1,2); hold on;
        plot(tSol,ySim(:,i),'k-','LineWidth',1)
        subplot(3,1,3); hold on;
        plot(tSol,uSim(:,i),'k-','LineWidth',1)
    end
end

subplot(3,1,1); hold on;
plot(tSol,xSol,'r-','LineWidth',3)
xlim(tSpan);
ylabel('x')
title('Compare simulations against reference trajectory')
subplot(3,1,2); hold on;
plot(tSol,ySol,'r-','LineWidth',3)
xlim(tSpan);
ylabel('y')
subplot(3,1,3); hold on;
plot(tSol,uSol,'r-','LineWidth',3)
xlim(tSpan);
ylabel('u')
xlabel('t')

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%    Computate the backward reachable set   --  Convex Hull Method        %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

nSamples = 500;  %How many simulations to run?
nPointsPerSample = 10;  %Used for computing reachable set

% What is "close enough" to the final state?
xRadFinal = 0.015;
yRadFinal = 0.015;

%Compute a random set of initial conditions
IC = randDisk(nSamples,[xRadFinal,yRadFinal]);

%Simulation runs backwards in time
tSpanFlip = linspace(tSpan(2),tSpan(1),nPointsPerSample);   %Run system backwards in time

%Create a data structure for storing the point cloud:
ReachCvx.t = ones(nSamples,1)*tSpanFlip;  % Time
ReachCvx.x = zeros(nSamples,nPointsPerSample);  % x position
ReachCvx.y = zeros(nSamples,nPointsPerSample);  % y position
ReachCvx.c(nPointsPerSample).x = []; %x contour
ReachCvx.c(nPointsPerSample).y = []; %x contour

%Run the simulation:
for i=1:nSamples
    idxLow = (i-1)*nPointsPerSample+1;
    idxUpp= i*nPointsPerSample;
    [~, yout] = ode45(userFunc,tSpanFlip,IC(:,i));
    ReachCvx.x(i,:) = yout(:,1);
    ReachCvx.y(i,:) = yout(:,2);
end

%Compute the convex hull at each time step
for i=1:nPointsPerSample
    k = convhull(ReachCvx.x(:,i),ReachCvx.y(:,i));
    ReachCvx.c(i).t = ReachCvx.t(k,i);
    ReachCvx.c(i).x = ReachCvx.x(k,i);
    ReachCvx.c(i).y = ReachCvx.y(k,i);
end

%Plot the reachable sets against time:
figure(6); clf; hold on;
colors = jet(nPointsPerSample);
for i=1:nPointsPerSample
    %Convex Hull
    plot3(ReachCvx.c(i).x, ReachCvx.c(i).y, ReachCvx.c(i).t,'color',colors(i,:));
    %Point Cloud:
    plot3(ReachCvx.x(:,i), ReachCvx.y(:,i), ReachCvx.t(:,i),'.','MarkerSize',10,'color',colors(i,:));
end

%plot the nominal trajectory
plot3(xSol,ySol,tSol,'k-','LineWidth',3);

title('Convex Hull approximation of the reachable set');
xlabel('x'); ylabel('y'); zlabel('time');
view(3);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%    Computate the backward reachable set   --  Front Propagation         %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

nSamples = 250;  %How many simulations to run?
nPointsPerSample = 10;  %Used for computing reachable set

% What is "close enough" to the final state?
xRadFinal = 0.015;
yRadFinal = 0.015;

%Compute initial conditions on the circle around the final set:
angle = linspace(0,2*pi,nSamples+1); angle(end) = [];
IC = [cos(angle); sin(angle)];
IC(1,:) = IC(1,:)*xRadFinal;
IC(2,:) = IC(2,:)*yRadFinal;

%Simulation runs backwards in time
tSpanFlip = linspace(tSpan(2),tSpan(1),nPointsPerSample);   %Run system backwards in time

%Create a data structure for storing the point cloud:
ReachFrt.t = ones(nSamples,1)*tSpanFlip;  % Time
ReachFrt.x = zeros(nSamples,nPointsPerSample);  % x position
ReachFrt.y = zeros(nSamples,nPointsPerSample);  % y position

%Run the simulation:
for i=1:nSamples
    idxLow = (i-1)*nPointsPerSample+1;
    idxUpp= i*nPointsPerSample;
    [~, yout] = ode45(userFunc,tSpanFlip,IC(:,i));
    ReachFrt.x(i,:) = yout(:,1);
    ReachFrt.y(i,:) = yout(:,2);
end

%Plot the reachable sets against time:
figure(7); clf; hold on;
colors = jet(nPointsPerSample);
idx = [1:size(ReachFrt.x,1),1];  %Make the contours wrap around
for i=1:nPointsPerSample
    %Front contour
    plot3(ReachFrt.x(idx,i), ReachFrt.y(idx,i), ReachFrt.t(idx,i),'color',colors(i,:));
    %Point Cloud:
    plot3(ReachFrt.x(:,i), ReachFrt.y(:,i), ReachFrt.t(:,i),'k.','MarkerSize',15);
    %Fill the central region:
    h = patch(ReachFrt.x(idx,i), ReachFrt.y(idx,i), ReachFrt.t(idx,i),colors(i,:));
end
plot3(xSol,ySol,tSol,'k-');

hp = findobj(gcf,'type','patch');
set(hp,'facealpha',0.7)
set(hp,'edgealpha',0.1)

title('Front Propagation approximation of the reachable set');
xlabel('x'); ylabel('y'); zlabel('time');
view(3);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%              Computation with the Level Set Toolbox                     %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

%This section of code requires the levelSetToolbox to operate:
%         http://www.cs.ubc.ca/~mitchell/ToolboxLS/
if exist('odeCFLset','file')
    %This section of code is still under development. As of 11/20/2014 the
    %following two scripts should run, but they are slow. It also seems
    %that there is some numerical dissapation, that is causing inaccurate
    %results for any reasonable level of discretization.

    %%%% reachableForward
    %%%% reachableForwardRel
end


function result = tetra_unit_sum ( func, norder, xtab, ytab, ztab, weight )

%*****************************************************************************80
%
%% TETRA_UNIT_SUM carries out a quadrature rule in the unit tetrahedron in 3D.
%
%  Integration region:
%
%    Points (X,Y,Z) such that
%
%      0 <= X,
%      0 <= Y,
%      0 <= Z, and
%      X + Y + Z <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function of three variables which is to be integrated,
%    of the form:
%      function value = func ( x, y, z )
%
%    Input, integer NORDER, the order of the rule.
%
%    Input, real XTAB(NORDER), YTAB(NORDER), ZTAB(NORDER), the
%    abscissas.
%
%    Input, real WEIGHT(NORDER), the weights.
%
%    Output, real RESULT, the approximate integral of the function.
%
  quad = 0.0E+00;

  for i = 1 : norder
    quad = quad + weight(i) * feval ( func, xtab(i), ytab(i), ztab(i) );
  end

  volume = tetra_unit_volume ( );
  result = quad * volume;

  return
end


function result = circle_rt_sum ( func, xc, yc, radius, nr, ra, rw, nt, ta, ...
  tw, zw )

%*****************************************************************************80
%
%% CIRCLE_RT_SUM applies an R, THETA product quadrature rule inside a circle.
%
%  Integration region:
%
%    Points (X,Y) such that:
%
%      (X-XC)**2 + (Y-YC)**2 <= RADIUS**2.
%
%  Discussion:
%
%    The product rule is assumed to be have the form:
%
%      Integral_Approx = ZW * F(XC,YC) +
%        Sum ( 1 <= IR <= NR ) Sum ( 1 <= IT <= NT )
%        RW(IR) * TW(IT) * F ( XC + R(IR) * RADIUS * Cos ( TA(IT) ),
%                              YC + R(IR) * RADIUS * Sin ( TA(IT) ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function of two variables which is to be integrated,
%    of the form:
%      function value = func ( x, y )
%
%    Input, real XC, YC, the coordinates of the center of
%    the circle.
%
%    Input, realn RADIUS, the radius of the circle.
%
%    Input, integer NR, the number of R abscissas.
%
%    Input, real RA(NR), RW(NR), the R abscissas and weights.
%
%    Input, integer NT, the number of Theta abscissas.
%
%    Input, real TA(NT), TW(NT), the THETA abscissas and weights.
%
%    Input, real ZW, the weight to use for the center.
%
%    Output, real RESULT, the approximate integral of the function.
%
  quad = 0.0E+00;

  if ( zw ~= 0.0E+00 )
    x = xc;
    y = yc;
    quad = quad + zw * feval ( func, x, y );
  end

  for it = 1 : nt
    rct = radius * cos ( ta(it) );
    rst = radius * sin ( ta(it) );
    for ir = 1 : nr
      x = xc + ra(ir) * rct;
      y = yc + ra(ir) * rst;
      quad = quad + tw(it) * rw(ir) * feval ( func, x, y );
    end
  end

  volume = circle_area_2d ( radius );
  result = quad * volume;

  return
end


function h=holder(tfr,f,n1,n2,t,pl)
%HOLDER	Estimate the Holder exponent through an affine TFR.
%	H=HOLDER(TFR,F,N1,N2,T) estimates the Holder exponent of a
%	function through an affine time-frequency representation of it,
%	and plots the frequency marginal and the regression line.
%
%	TFR : affine time-frequency representation.
%	F   : frequency values of the spectral analysis.
%	N1  : indice of the  minimum frequency for the linear regression.
%                                         (default : 1).
%	N2  : indice of the  maximum frequency for the linear regression.
%                                         (default : length(F)).
%	T : time vector. If T is omitted, the function returns the
%	    global estimate of the Holder exponent. Otherwise, it
%	    returns the local estimates H(T) at the instants specified
%	    in T.
%	H : output value (if T omitted) or vector (otherwise) containing
%	    the Holder estimate(s).
%
%	Example :
%	 S=altes(128); [TFR,T,F]=tfrscalo(S,1:128,8);
%	 H=holder(TFR,F,1,length(F));

%	P. Goncalves, October 1995
%	Copyright (c) 1995 Rice University
%
%  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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

clf;
tfr = abs(tfr) ;

if nargin<2,
 error('There must be at least 2 input parameters');
elseif nargin==2,
 n1=1; n2=length(f);
elseif nargin==3,
 n2=length(f);
end
if nargin<=5,
 pl=1;
else
 pl=0;
end

if nargin <= 4
  fmarg = mean(tfr.').' ;
  if pl, plot(log(f),log(fmarg)) , hold on; end;
  p = polyfit(log(f(n1:n2)),log(fmarg(n1:n2)),1) ;
  drt = p(1)*log(f)+p(2) ;
  if pl,
   plot(log(f),drt,'--g') ;
   legend('Frequency marginal','Regression line');
   plot(log(f([n1 n2])),log(fmarg([n1 n2])),'+r') ;
   xticklabels = round(logspace(log10(min(f)),log10(max(f)),4)*1000)./1000 ;
   set(gca,'XTick',log(xticklabels)) ;
   xlabel('frequency (logarithmically spaced)') ;
   hold off ;
  end
  h = (-p(1)-1)/2 ;
else
  if length(t) == 1
    if pl, plot(log(f),log(tfr(:,t))) ; hold on; end
    p = polyfit(log(f(n1:n2)),log(tfr(n1:n2,t)),1) ;
    drt = p(1)*log(f)+p(2) ;
    if pl,
     plot(log(f),drt,'--g') ;
     legend('Frequency marginal','Regression line');
     plot(log(f([n1 n2])),log(tfr([n1 n2],t)),'+r') ;
     xticklabels = round(logspace(log10(min(f)),log10(max(f)),4)*1000)./1000 ;
     set(gca,'XTick',log(xticklabels)) ;
     xlabel('frequency (logarithmically spaced)') ;
     hold off ;
    end
    h = (-p(1)-1)/2 ;
  elseif length(t) > 1
    [yt,xt] = size(t) ; if yt>xt, t = t.' ; end ;
    j = 1 ;
    for k = t,
      p = polyfit(log(f(n1:n2)),log(tfr(n1:n2,k)),1) ;
      h(j) = (-p(1)-1)/2 ; j = j + 1 ;
    end
    if pl,
     plot(t,h) ;grid
     title('Holder estimates at time instants T');
    end
  end,
end


%SerialLink.MANIPLTY Manipulability measure
%
% M = R.maniplty(Q, OPTIONS) is the manipulability index measure for the robot
% at the joint configuration Q.  It indicates dexterity, how isotropic the
% robot's motion is with respect to the 6 degrees of Cartesian motion.
% The measure is low when the manipulator is close to a singularity.
% If Q is a matrix M is a column vector of  manipulability
% indices for each pose specified by a row of Q.
%
% Two measures can be selected:
% - Yoshikawa's manipulability measure is based on the shape of the velocity
%   ellipsoid and depends only on kinematic parameters.
% - Asada's manipulability measure is based on the shape of the acceleration
%   ellipsoid which in turn is a function of the Cartesian inertia matrix and
%   the dynamic parameters.  The scalar measure computed here is the ratio of
%   the smallest/largest ellipsoid axis.  Ideally the ellipsoid would be
%   spherical, giving a ratio of 1, but in practice will be less than 1.
%
% Options::
% 'T'           compute manipulability for just transational motion
% 'R'           compute manipulability for just rotational motion
% 'yoshikawa'   use Asada algorithm (default)
% 'asada'       use Asada algorithm
%
% Notes::
% - by default the measure includes rotational and translational dexterity, but
%   this involves adding different units.  It can be more useful to look at the
%   translational and rotational manipulability separately.
%
% See also SerialLink.inertia, SerialLink.jacob0.

% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function [w,mx] = maniplty(robot, q, varargin)
	n = robot.n;

    opt.yoshikawa = true;
    opt.asada = false;
    opt.axes = {'all', 'T', 'R'};

    opt = tb_optparse(opt, varargin);

	if length(q) == robot.n,
		q = q(:)';
	end

	w = [];
    MX = [];

    if opt.yoshikawa
		for Q = q',
			w = [w; yoshi(robot, Q, opt)];
		end
	elseif opt.asada
		for Q = q',
            if nargout > 1
                [ww,mm] = asada(robot, Q);
                w = [w; ww];
                MX = cat(3, MX, mm);
            else
                w = [w; asada(robot, Q, opt)];
            end
		end
	end

    if nargout > 1
        mx = MX;
    end

function m = yoshi(robot, q, opt)
	J = jacob0(robot, q);
    switch opt.axes
    case 'T'
        J = J(1:3,:);
    case 'R'
        J = J(4:6,:);
    end
	m = sqrt(det(J * J'));

function [m, mx] = asada(robot, q, opt)
	J = jacob0(robot, q);

    if rank(J) < 6,
        warning('robot is in degenerate configuration')
        m = 0;
        return;
    end

    switch opt.axes
    case 'T'
        J = J(1:3,:)
    case 'R'
        J = J(4:6,:);
    end
	Ji = inv(J);
	M = inertia(robot, q);
	Mx = Ji' * M * Ji;
	e = eig(Mx(1:3,1:3));
	m = min(e) / max(e);

    if nargout > 1
        mx = Mx;
    end


function x = product_grid ( m, a, b, n, c, d )

%*****************************************************************************80
%
%% PRODUCT_GRID produces a product grid of points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real M, the number of grid points in X.
%
%    Input, real A, B, the X limits.
%
%    Input, real N, the number of grid points in Y.
%
%    Input, real C, D, the Y limits.
%
%    Output, real X(M*N,2), the points of the product grid.
%
  x = zeros ( m * n, 2 );

  k = 0;
  for i = 1 : m
    for j = 1 : n
      k = k + 1;
      x(k,1) = ( ( m - i ) * a + ( i - 1 ) * b ) / ( m - 1 );
      x(k,2) = ( ( n - j ) * c + ( j - 1 ) * d ) / ( n - 1 );
    end
  end

  return
end


% Fast linear interpolation of ordered values (one dimendionnal)
%
% This matlab version is made to illustrate the behavior of the C version.
% Do not expect speed from this Matlab version  !
% Also, if this function is updated, its C version has to be updated
% accordingly.
%
% Note tha the C version makes absolutely NO check at all.
% Thus, it can crash very easily.
%
% Build the C version using
%  $ mex interp1ordered.c
%
% Input
%  x   ; [Nx1] Ordered reference abscissa
%  y   ; [Nx1] Ordered reference ordinates
%  xi  : [Mx1] Ordered abscissa where to interpolate
%  yid : [Mx1] Default out of bounds value
%
% Output
%  yi  : Interpolated output values
%
% Copyright (c) 2012 University of Crete - Computer Science Department (UOC-CSD)
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>

function yi = interp1ordered(x, y, xi, yid)

    yi = yid*ones(size(xi));

    i=1;
    j=1;

    while xi(i)<x(1)
        yi(i) = yid;
        i=i+1;
    end

    while i<=length(xi)
        while j<length(x) && x(j+1)<xi(i);
            j=j+1;
        end
        if j==length(x); break; end

        g = (xi(i)-x(j))./(x(j+1)-x(j));

        yi(i) = y(j,:)*(1-g) + y(j+1,:)*g;

        i=i+1;
    end

    if i<=length(xi)
        yi(i:end) = yid;
    end

return


function trans = createScaling3d(varargin)
%CREATESCALING3D Create the 4x4 matrix of a 3D scaling.
%
%   TRANS = createScaling3d(S);
%   returns the scaling transform corresponding to a scaling factor S in
%   each direction. S can be a scalar, or a 1-by-3 vector containing the
%   scaling factor in each direction.
%
%   TRANS = createScaling3d(SX, SY, SZ);
%   returns the scaling transform corresponding to a different scaling
%   factor in each direction.
%
%   The returned matrix has the form :
%   [SX  0  0  0]
%   [ 0 SY  0  0]
%   [ 0  0 SZ  0]
%   [ 0  0  0  0]
%
%   See also
%   transforms3d, transformPoint3d, transformVector3d, createTranslation3d,
%   createRotationOx, createRotationOy, createRotationOz
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2006-04-20
% Copyright 2006-2022 INRA - TPV URPOI - BIA IMASTE

%% default arguments
sx = 1;
sy = 1;
sz = 1;
center = [0 0 0];

%% process input parameters
if nargin == 1
    % only one argument -> scaling factor
    [sx, sy, sz]= parseScalingFactors(varargin{1});

elseif nargin == 2
    % 2 arguments, giving center and uniform scaling
    center = varargin{1};
    [sx, sy, sz]= parseScalingFactors(varargin{2});

elseif nargin == 3
    % 3 arguments, giving scaling in each direction
    sx = varargin{1};
    sy = varargin{2};
    sz = varargin{3};

elseif nargin == 4
    % 4 arguments, giving center and scaling in each direction
    center = varargin{1};
    sx = varargin{2};
    sy = varargin{3};
    sz = varargin{4};
end

%% create the scaling matrix
trans = [...
    sx 0 0 center(1)*(1-sx);...
    0 sy 0 center(2)*(1-sy);...
    0 0 sz center(3)*(1-sz);...
    0 0 0 1];

%% Helper function
function [sx, sy, sz] = parseScalingFactors(var)

if length(var)==1
    % same scaling factor in each direction
    sx = var;
    sy = var;
    sz = var;
elseif length(var)==3
    % scaling is a vector, giving different scaling in each direction
    sx = var(1);
    sy = var(2);
    sz = var(3);
else
    error('wrong size for first parameter of "createScaling3d"');
end


function x = nco_abscissas_ab ( a, b, n )

%*****************************************************************************80
%
%% NCO_ABSCISSAS_AB computes the Newton Cotes Open abscissas for [A,B].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the endpoints of the interval.
%
%    Input, integer N, the order of the rule.
%
%    Output, real X(N), the abscissas.
%
  for i = 1 : n
    x(i) = ( ( n - i + 1 ) * a   ...
           + (     i     ) * b ) ...
           / ( n     + 1 );
  end

  return
end


function mono_between_enum_test ( )

%*****************************************************************************80
%
%% MONO_BETWEEN_ENUM_TEST tests MONO_BETWEEN_ENUM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 November 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MONO_BETWEEN_ENUM_TEST\n' );
  fprintf ( 1, '  MONO_BETWEEN_ENUM can enumerate the number of monomials\n' );
  fprintf ( 1, '  in M variables, of total degree between N1 and N2.\n' );

  m = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Using spatial dimension M = %d\n', m );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   N2:' );
  for n2 = 0 : 8
    fprintf ( 1, '  %4d', n2 );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '  N1 +------------------------------------------------------\n' );
  for n1 = 0 : 8
    fprintf ( 1, '  %2d |', n1 );
    for n2 = 0 : 8
      v = mono_between_enum ( m, n1, n2 );
      fprintf ( 1, '  %4d', v );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function [h] = fdr(p, q)

% FDR false discovery rate
%
% Use as
%   h = fdr(p, q)
%
% This implements
%   Genovese CR, Lazar NA, Nichols T.
%   Thresholding of statistical maps in functional neuroimaging using the false discovery rate.
%   Neuroimage. 2002 Apr;15(4):870-8.
%
% There are two types of FDR correction (Benjamini-Hochberg & Benjamini-Yekutieli), of
% which the second is currently implemented.

% Copyright (C) 2005-2015, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% convert the input into a row vector
dim = size(p);
p = reshape(p, 1, numel(p));

% sort the observed uncorrected probabilities
[ps, indx] = sort(p);

% count the number of voxels
V = length(p);

% compute the threshold probability for each voxel
pi = ((1:V)/V)  * q / c(V);

if any(ps<=pi)
  h = ps<=(max(ps(ps<=pi)));
else
  h = false(size(ps));
end

% undo the sorting
[dum, unsort] = sort(indx);
h = h(unsort);

% convert the output back into the original format
h = reshape(h, dim);

function s = c(V)
% See Genovese, Lazar and Holmes (2002) page 872, second column, first paragraph
if V<1000
  % compute it exactly
  s = sum(1./(1:V));
else
  % approximate it
  s = log(V) + 0.57721566490153286060651209008240243104215933593992359880576723488486772677766467093694706329174674951463144724980708248096050401448654283622417399764492353625350033374293733773767394279259525824709491600873520394816567;
end


function [ po, pc, pe ] = lagrange_partial2 ( d, n, r, nd, xd )

%*****************************************************************************80
%
%% LAGRANGE_PARTIAL2: Partial Lagrange polynomial basis from data.
%
%  Discussion:
%
%    This function represents algorithm 4.1 in the reference,
%    modified for the case where the number of data points is less
%    than the dimension of the desired polynomial space,
%    with the further modification that a form of "pivoting" is used
%    to select the next polynomial as the one with maximum absolute
%    value at the current node.
%
%    This function is given XD, a set of ND distinct data points in a
%    D dimensional space, and returns information defining a set of
%    ND Lagrange polynomials L(i)(X) with the property that:
%
%      L(i)(XD(j)) = delta(i,j)
%
%    This function is used in cases where ND, the number of data points,
%    is less than or equal to R, the dimension of the space of polynomials
%    in D dimensions and total degree N or less, that is:
%
%      ND <= R = Choose ( N + D, N )
%
%    There will be ND polynomials returned.  Each polynomial can have
%    as many as R coefficients.
%
%    Each polynomial is given as a vector, with each entry corresponding
%    to a nonzero coefficient.  In particular, for polynomial L(i)(X):
%
%      PO(i) is the order, that is, the number of nonzero coefficients;
%      PC(i,j), for 1 <= j <= PO(i), is the coefficient of the J-th term.
%      PE(i,j), for 1 <= j <= PO(i), encodes the exponents of the J-th term.
%
%    The exponent codes are a compact way of recording the exponent vector
%    associated with each monomial.  If PE(i,j) = k, then the corresponding
%    vector of D exponents can be determined by:
%
%      E = mono_unrank_grlex ( D, k );
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Tomas Sauer, Yuan Xu,
%    On multivariate Lagrange interpolation,
%    Mathematics of Computation,
%    Volume 64, Number 211, July 1995, pages 1147-1170.
%
%  Parameters:
%
%    Input, integer D, the spatial dimension.
%
%    Input, integer N, the maximum total degree.
%
%    Input, integer R, the number of monomials in D dimensions
%    of total degree N or less.
%
%    Input, integer ND, the number of data points.
%    It must be the case that ND <= R.
%
%    Input, real XD(D,ND), the data points, which must be distinct.
%
%    Output, integer PO(ND), the order (number of nonzero coefficients) for the
%    Lagrange basis polynomials.
%
%    Output, real PC(ND,R), the coefficients for the
%    Lagrange basis polynomials.
%
%    Output, integer PE(ND,R), the exponent indices for the
%    Lagrange basis polynomials.
%

%
%  Verify that R is correct.
%
  if ( r ~= mono_upto_enum ( d, n ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LAGRANGE_PARTIAL2 - Fatal error!\n' );
    fprintf ( 1, '  The value R is not correct.\n' );
    error ( 'LAGRANGE_PARTIAL2 - Fatal error!' );
  end

  if ( r < nd )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LAGRANGE_PARTIAL2 - Fatal error!\n' );
    fprintf ( 1, '  The value R = %d is less than ND = %d.\n', r, nd );
    error ( 'LAGRANGE_PARTIAL2 - Fatal error!' );
  end
%
%  Verify that the points are sufficiently distinct.
%
  [ d_min, d_max ] = r8col_separation ( d, nd, xd );
  d_tol = sqrt ( eps );

  if ( d_min < d_tol )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LAGRANGE_PARTIAL2 - Fatal error!\n' );
    fprintf ( 1, '  Some points are too close!\n' );
    fprintf ( 1, '  Minimum data point separation is = %g.\n', d_min );
    error ( 'LAGRANGE_PARTIAL2 - Fatal error!' );
  end
%
%  Initialize the polynomials Q to be all monomials of degree exactly N.
%
  qo = zeros ( r, 1 );
  qc = zeros ( r, r );
  qe = zeros ( r, r );

  for k = 1 : r
    qo(k,1) = 1;
    qc(k,1) = 1.0;
    qe(k,1) = k;
  end
%
%  Now set up the P polynomials.
%
  po = zeros ( nd, 1 );
  pc = zeros ( nd, r );
  pe = zeros ( nd, r );

  for k = 1 : nd
%
%  Find the polynomial Q(K:R)(X) which is most nonzero at X(K).
%
    i = r + 1;
    value_max = 0.0;

    for j = k : r

      o = qo(j);
      value = polynomial_value ( d, o, qc(j,1:o), qe(j,1:o), 1, xd(1:d,k) );

      if ( abs ( value_max ) <= abs ( value ) )
        i = j;
        value_max = value;
      end

    end

    if ( i == r + 1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'LAGRANGE_PARTIAL2 - Fatal error!\n' );
      fprintf ( 1, '  I = R+1.\n' );
      fprintf ( 1, '  No polynomial was nonzero at the current node.\n' );
      error ( 'LAGRANGE_PARTIAL2 - Fatal error!' );
    end

    value = value_max;
%
%  Define P(K)(X) = Q(I)(X) / Q(I)(X(k)
%
    o = qo(i);
    po(k) = qo(i);
    pc(k,1:o) = qc(i,1:o) / value;
    pe(k,1:o) = qe(i,1:o);
%
%  Modify P(1:k-1)(X).
%
    for j = 1 : k - 1

      oj = po(j);
      ok = po(k);

      value = polynomial_value ( d, oj, pc(j,1:oj), pe(j,1:oj), 1, xd(1:d,k) );

      [ o, c, e ] = polynomial_axpy ( - value, ok, pc(k,1:ok), pe(k,1:ok), ...
        oj, pc(j,1:oj), pe(j,1:oj) );

      po(j) = o;
      pc(j,1:o) = c(1:o)';
      pe(j,1:o) = e(1:o)';

    end
%
%  Modify Q(I:downto:K+1)
%
    for j = i : -1 : k + 1

      oj = qo(j-1);
      ok = po(k);

      value = polynomial_value ( d, oj, qc(j-1,1:oj), qe(j-1,1:oj), ...
        1, xd(1:d,k) );

      [ o, c, e ] = polynomial_axpy ( - value, ok, pc(k,1:ok), pe(k,1:ok), ...
        oj, qc(j-1,1:oj), qe(j-1,1:oj) );

      qo(j) = o;
      qc(j,1:o) = c(1:o)';
      qe(j,1:o) = e(1:o)';

    end
%
%  Modify Q(I+1:R)
%
    for j = i + 1 : r

      oj = qo(j);
      ok = po(k);

      value = polynomial_value ( d, oj, qc(j,1:oj), qe(j,1:oj), ...
        1, xd(1:d,k) );

      [ o, c, e ] = polynomial_axpy ( - value, ok, pc(k,1:ok), pe(k,1:ok), ...
        oj, qc(j,1:oj), qe(j,1:oj) );

      qo(j) = o;
      qc(j,1:o) = c(1:o)';
      qe(j,1:o) = e(1:o)';

    end

  end
%
%  Get rid of tiny coefficients.
%
  for i = 1 : nd
    oi = po(i);
    [ o, c, e ] = polynomial_compress ( ...
      po(i), pc(i,1:oi), pe(i,1:oi) );
    po(i) = o;
    pc(i,1:o) = c(1:o)';
    pe(i,1:o) = e(1:o)';
  end

  return
end


function [Zc mu] = myCenter(Z)
%--------------------------------------------------------------------------
% Syntax:       Zc = myCenter(Z);
%               [Zc mu] = myCenter(Z);
%
% Inputs:       Z is an (d x n) matrix containing n samples of a
%               d-dimensional random vector
%
% Outputs:      Zc is the centered version of Z
%
%               mu is the (d x 1) sample mean of Z
%
% Description:  This function returns the centered (i.e., zero mean)
%               version of the input samples
%
%               NOTE: Z = Zc + repmat(mu,1,n);
%
% Author:       Brian Moore
%               brimoor@umich.edu
%
% Date:         April 26, 2015
%--------------------------------------------------------------------------

% Compute sample mean
mu = mean(Z,2);

% Subtract mean
Zc = bsxfun(@minus,Z,mu);


function [xd,xn,option] = denoise(x,h,type,option)
%    [xd,xn,option] = denoise(x,h,type,option);
%
%    DENOISE is a generic program for wavelet based denoising.
%    The program will denoise the signal x using the 2-band wavelet
%    system described by the filter h using either the traditional
%    discrete wavelet transform (DWT) or the linear shift invariant
%    discrete wavelet transform (also known as the undecimated DWT
%    (UDWT)).
%
%    Input:
%       x         : 1D or 2D signal to be denoised
%       h         : Scaling filter to be applied
%       type      : Type of transform (Default: type = 0)
%                   0 --> Discrete wavelet transform (DWT)
%                   1 --> Undecimated DWT (UDWT)
%       option    : Default settings is marked with '*':
%                   *type = 0 --> option = [0 3.0 0 0 0 0]
%                   type = 1 --> option = [0 3.6 0 1 0 0]
%       option(1) : Whether to threshold low-pass part
%                   0 --> Don't threshold low pass component
%                   1 --> Threshold low pass component
%       option(2) : Threshold multiplier, c. The threshold is
%                   computed as:
%                     thld = c*MAD(noise_estimate)).
%                   The default values are:
%                     c = 3.0 for the DWT based denoising
%                     c = 3.6 for the UDWT based denoising
%       option(3) : Type of variance estimator
%                   0 --> MAD (mean absolute deviation)
%                   1 --> STD (classical numerical std estimate)
%       option(4) : Type of thresholding
%                   2 --> Soft thresholding
%                   1 --> Hard thresholding
%       option(5) : Number of levels, L, in wavelet decomposition. By
%                   setting this to the default value '0' a maximal
%                   decomposition is used.
%       option(6) : Actual threshold to use (setting this to
%                   anything but 0 will mean that option(3)
%                   is ignored)
%
%    Output:
%       xd     : Estimate of noise free signal
%       xn     : The estimated noise signal (x-xd)
%       option : A vector of actual parameters used by the
%                program. The vector is configured the same way as
%                the input option vector with one added element
%                option(7) = type.
%
%  HERE'S AN EASY WAY TO RUN THE EXAMPLES:
%  Cut-and-paste the example you want to run to a new file
%  called ex.m, for example. Delete out the % at the beginning
%  of each line in ex.m (Can use search-and-replace in your editor
%  to replace it with a space). Type 'ex' in matlab and hit return.
%
%    Example 1:
%       h = daubcqf(6); [s,N] = makesig('Doppler'); n = randn(1,N);
%       x = s + n/10;     % (approximately 10dB SNR)
%       figure;plot(x);hold on;plot(s,'r');
%
%       %Denoise x with the default method based on the DWT
%       [xd,xn,opt1] = denoise(x,h);
%       figure;plot(xd);hold on;plot(s,'r');
%
%       %Denoise x using the undecimated (LSI) wavelet transform
%       [yd,yn,opt2] = denoise(x,h,1);
%       figure;plot(yd);hold on;plot(s,'r');
%
% Example 2: (on an image)
%      h = daubcqf(6);  load lena;
%      noisyLena = lena + 25 * randn(size(lena));
%      figure; colormap(gray); imagesc(lena); title('Original Image');
%       figure; colormap(gray); imagesc(noisyLena); title('Noisy Image');
%       Denoise lena with the default method based on the DWT
%      [denoisedLena,xn,opt1] = denoise(noisyLena,h);
%      figure; colormap(gray); imagesc(denoisedLena); title('denoised Image');
%
%
%    See also: mdwt, midwt, mrdwt, mirdwt, SoftTh, HardTh, setopt
%
%Author: Jan Erik Odegard  <odegard@ece.rice.edu>

if(nargin < 2)
  error('You need to provide at least 2 inputs: x and h');
end;
if(nargin < 3),
  type = 0;
  option = [];
elseif(nargin < 4)
  option = [];
end;
if(isempty(type)),
  type = 0;
end;
if(type == 0),
  default_opt = [0 3.0 0 2 0 0];
elseif(type == 1),
  default_opt = [0 3.6 0 1 0 0];
else
  error(['Unknown denoising method',10,...
	  'If it is any good we need to have a serious talk :-)']);
end;
option = setopt(option,default_opt);
[mx,nx] = size(x);
dim = min(mx,nx);
if(dim == 1),
  n = max(mx,nx);
else
  n = dim;
end;
if(option(5) == 0),
  L = floor(log2(n));
else
  L = option(5);
end;
if(type == 0), 			% Denoising by DWT
  xd = mdwt(x,h,L);
  if (option(6) == 0),
    tmp = xd(floor(mx/2)+1:mx,floor(nx/2)+1:nx);
    if(option(3) == 0),
      thld = option(2)*median(abs(tmp(:)))/.67;
    elseif(option(3) == 1),
      thld = option(2)*std(tmp(:));
    else
      error('Unknown threshold estimator, Use either MAD or STD');
    end;
  else
    thld = option(6);
  end;
  if(dim == 1)
    ix = 1:n/(2^L);
    ykeep = xd(ix);
  else
    ix = 1:mx/(2^L);
    jx = 1:nx/(2^L);
    ykeep = xd(ix,jx);
  end;
  if(option(4) == 2),
    xd = SoftTh(xd,thld);
  elseif(option(4) == 1),
    xd = HardTh(xd,thld);
  else
    error('Unknown threshold rule. Use either Soft (2) or Hard (1)');
  end;
  if (option(1) == 0),
    if(dim == 1),
      xd(ix) = ykeep;
    else
      xd(ix,jx) = ykeep;
    end;
  end;
  xd = midwt(xd,h,L);
elseif(type == 1), 			% Denoising by UDWT
  [xl,xh] = mrdwt(x,h,L);
  if(dim == 1),
    c_offset = 1;
  else
    c_offset = 2*nx + 1;
  end;
  if (option(6) == 0),
    tmp = xh(:,c_offset:c_offset+nx-1);
    if(option(3) == 0),
      thld = option(2)*median(abs(tmp(:)))/.67;
    elseif(option(3) == 1),
      thld = option(2)*std(tmp(:));
    else
      error('Unknown threshold estimator, Use either MAD or STD');
    end;
  else
    thld = option(6);
  end;
  if(option(4) == 2),
    xh = SoftTh(xh,thld);
    if(option(1) == 1),
      xl = SoftTh(xl,thld);
    end;
  elseif(option(4) == 1),
    xh = HardTh(xh,thld);
    if(option(1) == 1),
      xl = HardTh(xl,thld);
    end;
  else
    error('Unknown threshold rule. Use either Soft (2) or Hard (1)');
  end;
  xd = mirdwt(xl,xh,h,L);
else 					% Denoising by unknown method
  error(['Unknown denoising method',10,...
         'If it is any good we need to have a serious talk :-)']);
end;
option(6) = thld;
option(7) = type;
xn = x - xd;


function alpha = normalizeAngle(alpha, varargin)
%NORMALIZEANGLE  Normalize an angle value within a 2*PI interval.
%
%   ALPHA2 = normalizeAngle(ALPHA);
%   ALPHA2 is the same as ALPHA modulo 2*PI and is positive.
%
%   ALPHA2 = normalizeAngle(ALPHA, CENTER);
%   Specifies the center of the angle interval.
%   If CENTER==0, the interval is [-pi ; +pi]
%   If CENTER==PI, the interval is [0 ; 2*pi] (default).
%
%   Example:
%   % normalization between 0 and 2*pi (default)
%   normalizeAngle(5*pi)
%   ans =
%       3.1416
%
%   % normalization between -pi and +pi
%   normalizeAngle(7*pi/2, 0)
%   ans =
%       -1.5708
%
%   See also
%   vectorAngle, lineAngle
%

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Created: 2008-03-10,    using Matlab 7.4.0.287 (R2007a)
% Copyright 2008 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

% HISTORY
% 2010-03-31 rename as normalizeAngle, and add psb to specify interval
%   center

center = pi;
if ~isempty(varargin)
    center = varargin{1};
end

alpha = mod(alpha-center+pi, 2*pi) + center-pi;


function [ x, seed ] = normal_ms_sample ( mu, sigma, seed )

%*****************************************************************************80
%
%% NORMAL_MS_SAMPLE samples the Normal MS PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real MU, SIGMA, the parameters of the PDF.
%    0.0 < SIGMA.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  [ y, seed ] = normal_01_sample ( seed );

  x = mu + sigma * y;

  return
end


function [Ec,Rad,Diam,Cv,Pv]=grEccentricity(E)
% Function Ec=grEccentricity(E) find the (weighted)
% eccentricity of all vertexes of graph.
% Input parameter:
%   E(m,2) or (m,3) - the edges of graph and their weight;
%     1st and 2nd elements of each row is numbers of vertexes;
%     3rd elements of each row is weight of arrow;
%     m - number of arrows.
%     If we set the array E(m,2), then all weights is 1.
% Output parameter:
%   Ec(1,n) - the (weighted) eccentricity of all vertexes.
% [Ec,Rad,Diam]=grEccentricity(E) find also the radius Rad and
%   diameter Diam of the graph (the scalars).
% [Ec,Rad,Diam,Cv,Pv]=grEccentricity(E) find also
%   the center vertexes Cv and the periphery vertexes Pv
%   of the graph (the vector-rows with numbers of vertexes).
% Author: Sergiy Iglin
% e-mail: siglin@yandex.ru
% personal page: http://iglin.exponenta.ru

if nargin<1,
  error('There are no input data!')
end
dSP=grDistances(E); % the matrix of distances
Ec=max(dSP); % the eccentricity of all vertexes
Rad=min(Ec); % the radius
Diam=max(Ec); % the diameter
Cv=find(Ec==Rad); % the center vertexes of the graph
Pv=find(Ec==Diam); % the periphery vertexes of the graph
return


function [normal, d] = plot_line(p1, p2, p3)

% This function plots a line from three points.
% I/P arguments:
%   p1, p2, p3 eg, p1 = [x y z]
%
%
% O/P is:
% normal: it contains a,b,c coeff , normal = [a b c]
% d : coeff

normal = cross(p1 - p2, p1 - p3);
d = p1(1)*normal(1) + p1(2)*normal(2) + p1(3)*normal(3);
d = -d;
x = -100:100; y = -100:100;
[X,Y] = meshgrid(x,y);
Z = (-d - (normal(1)*X) - (normal(2)*Y))/normal(3);
mesh(X,Y,Z)


function I=montec2vvar(g,cx,dx,a,b,c,d,n)

%I=montec2vvar('g',cx,dx,a,b,c,d,n)
%Calculates integral on no rectangular space using monte-carlo method
%a,b, fixed boundaries
%cx,dx, variable boundaries (of x)
%c,d fixed boundaries (of y) that form a rectangular space with a and b
%boundaries.
t=rand(1,n);
u=rand(1,n);
x=a+t.*(b-a);			%extremos a,b fijos
y=c+u.*(d-c);			%extremos c,d variabales

cont=0;
s=0;

for i=1:n
   if (y(i)>feval(cx,x(i))&&y(i)<feval(dx,x(i)))
      cont=cont+1;
      fx=feval(g,x(i),y(i));
      s=s+fx;
   end
end

area=(simpson(dx,a,b,20)-simpson(cx,a,b,20))
I=((area./cont).*s);


function p = motionProfile(profileType, t, x, ptotal)
% motionProfile  Compute motion profiles for the crane block
%   PROF = motionProfile(PROFILETYPE, T, X, PTOTAL) returns the profile of
%     the crane block.
%     PROFILETYPE : 'acceleration', 'velocity', 'profile'
%     T           : time vector
%     X           : 3-element vector [tp1, tp2, tf]
%     PTOTAL      : total displacement
%
%   The acceleration profile for this problem is a "bang-coast-bang"
%   profile.
%
%  Example:
%    x = [10 5 20];
%    ptotal = 50;
%    t = 0:.01:30;
%
%    acc = motionProfile('accel', t, x, ptotal);
%    vel = motionProfile('vel', t, x, ptotal);
%    pos = motionProfile('pos', t, x, ptotal);

% Copyright 2011 The MathWorks, Inc.

error(nargchk(4, 4, nargin, 'struct'));

profileType = validatestring(profileType, ...
   {'acceleration', 'velocity', 'position'}, mfilename, 'PROFILETYPE');

tp1 = x(1); tp2 = x(2); tf = x(3);

% Pulse heights
p1 = 2*ptotal/(tp1*(2*tf-tp1-tp2));
p2 = tp1/tp2*p1;

switch profileType
   case 'acceleration'
      p = zeros(size(t));

      % Acceleration phase
      p(t>=0 & t<tp1) = p1;

      % Deceleration phase
      p(t>=tf-tp2 & t<tf) = -p2;

   case 'velocity'
      p = zeros(size(t));

      % Acceleration phase
      idx1 = t>=0 & t<tp1;
      p(idx1) = t(idx1)*p1;

      % Coast
      p(t>=tp1 & t<tf-tp2) = tp1*p1;

      % Deceleration phase
      idx2 = t>=tf-tp2 & t<tf;
      p(idx2) = tp1*p1 - (t(idx2)-t(find(idx2, 1, 'first')))*p2;

   case 'position'
      p = ptotal*ones(size(t));

      % Acceleration phase
      idx1 = t>=0 & t<tp1;
      p(idx1) = 0.5*p1*t(idx1).^2;

      % Coast phase
      idx2 = t>=tp1 & t<tf-tp2;
      idx2_f = find(idx2, 1, 'first');
      p(idx2) = 0.5*p1*t(idx2_f).^2 + (t(idx2)-t(idx2_f))*tp1*p1;

      % Deceleration phase
      idx3 = t>=tf-tp2 & t<tf;
      idx3_f = find(idx3, 1, 'first');
      p(idx3) = 0.5*p1*t(idx2_f).^2 + (t(idx3_f)-t(idx2_f))*tp1*p1 +...
         tp1*p1*(t(idx3)-t(idx3_f)) - 0.5*p2*(t(idx3)-t(idx3_f)).^2;

end


function [v, d, m] = scimat_pca(scimat)
% SCIMAT_PCA Principal Principal Component Analysis of the selected points
% in a SCIMAT segmentation mask.
%
% [V, D, M] = scimat_pca(SCIMAT)
%
%   This function computes Principal Component Analysis (PCA) on the
%   collection of points in a SCIMAT segmentation mask.
%
%   SCIMAT is a struct with the segmentation (see "help scimat" for
%   details).
%
%   V is a matrix with the column eigenvectors ordered in decreasing order
%   of the corresponding eigenvalues in D.
%
%   M is a 3-vector with the coordinates of the dataset centroid. The
%   centroid is the centre of the coordinate system defined by the
%   eigenvectors, and the centre of rotation if the data is rotated.
%
%   Assuming a 3D volume, V is a (3, 3)-matrix and D a 3-vector.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2010,2014 University of Oxford
% Version: 0.2.1
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(1, 1) ;
nargoutchk(0, 3) ;

% squeeze SCIMAT variables, if necessary
scimat = scimat_squeeze(scimat);

% extract linear indices of voxels in the segmentation
idx = find(scimat.data);

% get volume size
sz = size(scimat.data);

% convert linear index to multiple subscripts
[ir, ic, iz] = ind2sub(sz, idx);

% convert indices to real world coordinates
x = scimat_index2world([ir, ic, iz], scimat);

% compute centroid
m = mean(x);

% compute PCA
[v, d] = pts_pca(x');


function varargout=gabor3_fwb(aspect,theta,bw,psi,sigma,sz)
% Returns 3D gabor filter.
% gb=GABOR_FWB(aspect,theta,bw,psi,sigma,sz)
%
% [aspecta, aspectb]
%        = 2 element array giving aspect ratios for 2 minor axis
%           (eg: [0.5, 1], for major < minoraxis1, major = minoraxi2)
% [theta,phi]
%        = yaw and pitch of major axis (0-2*pi)
%           roll isn't implemented, sorry.
% bw     = spatial bandwidth in pixels (decreasing fine detail,), (eg: >=1)
%               scales the frequency of the cosine modulation
% psi,   = phase shift, [optional, default: 0]
% sigma  = scales the falloff of the gaussian, (must be >=2) [default: = bw]
%              + can set to 'auto' to maintain default functionality
% [x y z] = size of gabor kernel created  [optional, size set automatically
%           to 3 standard deviations of gaussian kernel]

% Frederick Bryan adapted from gabor_fn.m
% July 2013
% Vanderbilt Univ

% To Do
% implement roll:
% This isn't hard as far as the meshgrid goes. All that needs to be done for that is to
% uncomment the appropriate lines. I just didn't want to figure out the math
% for the auto-sizing option. (if nargin < 6)
%

% handle mandatory args
if numel(aspect) ~= 2;
    error('1st argument (aspect) must be 2 element array');
end
if numel(theta) ~=2;
    error('2nd argument ([theta,phi]) must be 2 element array');
end

% handle optional inputs
if nargin<4
    psi = [0 0];
end
if nargin<5
    sigma = 'auto';
end
% allow 'auto' sizing of guassian kernel
if strcmp(sigma,'auto');
    sigma = bw;
end
if nargin<6
    % figure out size
    phi = theta(2);
    theta = theta(1);
    len1 = 3*sigma; % length along theta direction
    len2 = len1/aspect(1);
    len3 = len1/aspect(2);
    sz(1) = (len1*cos(theta)+len2*sin(theta)); % column/x dimension
    sz(2) = (len1*sin(theta)+len2*cos(theta)); % row/y direction
    sz(1) = round(sz(1)*cos(phi)+len3*sin(phi));
    sz(2) = round(sz(2));
    sz(3) = round(sz(1)*sin(phi)+len3*cos(phi));
end

% handle incorrectly given optional args

if length(sz)<2; % allow just one number to be given for size
    sz(2) = sz(1);
    sz(3) = sz(1);
end
% keyboard;
sx = sz(1);
sy = sz(2);
sz = sz(3);

% figured out size above, now make matrix of points
[x y z]=meshgrid(-sx:sx, sy:-1:-sy, -sz:sz); % note that y goes backwards

% rotate reference frame to point in theta direction
% http://en.wikipedia.org/wiki/Rotation_matrix
% yaw - rotation about z
xp = x*cos(theta)+y*sin(theta);
yp = -x*sin(theta)+y*cos(theta);
zp = z;
% pitch - rotation about y
xp = xp*cos(phi)-zp*sin(phi);
yp = yp;
zp = xp*sin(phi)+zp*cos(phi);
% % roll - rotation about x - unimplemented, requires "roll" angle
% xp = xp;
% yp = yp*cos(roll) + zp*sin(roll);
% zp = -yp*sin(roll) + zp*cos(roll);

% create gaussian pointing in theta direction with size determined by
%   aspect ration
sigmajor = sigma;
sigminor1 = sigma/aspect(1);
sigminor2 = sigma/aspect(2);
h1 = exp(-(xp.^2/sigmajor^2+yp.^2/sigminor1^2+zp.^2/sigminor2^2));

% multiply by cosine with appropriate bw
F=1/bw;  % Frequency
h2 = cos(2*pi*(F*xp)+psi);

g = h1.*h2;

if nargout>0
    varargout{1} = g;
end


function [ errors ] = test_operators( )

errors = 0;

errors = errors + test_fourier_basis_ring();
errors = errors + test_fourier_basis_ring_advanced();

%errors = errors + test_translate();
errors = errors + test_modulate();

errors = errors + test_gwft();
errors = errors + test_gft();
errors = errors + test_ngwft();

errors = errors + test_divgradient();
errors = errors + test_gradient();
errors = errors + test_gradient2();
errors = errors + test_gradient_mat();

errors = errors + test_directed();
errors = errors + test_advection();

errors = errors + test_div();
errors = errors + test_div2();
errors = errors + test_div3();
errors = errors + test_laplacian();

errors = errors + test_kron_reduction();

end

function s = eigvec_ring(input,N)
s = zeros(N,numel(input));
n = (0:(N-1))';
for ind = 1:numel(input)
    if mod(input(ind),2) % odd
        s(:,ind) = 1/sqrt(N)*exp(2*pi*1i*(N-(input(ind)+1)/2)*n/N);
    else % even
        s(:,ind) = 1/sqrt(N)*exp(pi*1i*input(ind)*n/N);
    end
end

end

function s = eigval_ring(input,N)
s = zeros(size(input));
for ind = 1:numel(input)
    if mod(input(ind),2) % odd
        s(ind) = 1-cos(pi * (input(ind)+1)/N );
    else % even
        s(ind) = 1-cos(pi * (input(ind))/N );
    end
end

end

function errors = test_fourier_basis_ring_advanced()
errors = 0;
N = 10;
e = 2*eigval_ring(0:N-1,N)';
U = eigvec_ring(0:N-1,N);
G = gsp_ring(N);
G = gsp_compute_fourier_basis(G);
errors = errors +gsp_assert_test(G.e,e,1e-14,'test ring eigenvalues N=10');
errors = errors +gsp_assert_test(G.U,U,1e-14,'test ring eigenvector N=10');

N = 11;
e = 2*eigval_ring(0:N-1,N)';
U = eigvec_ring(0:N-1,N);
G = gsp_ring(N);
G = gsp_compute_fourier_basis(G);
errors= errors +gsp_assert_test(G.e,e,1e-14,'test ring eigenvalues N=11');
errors = errors +gsp_assert_test(G.U,U,1e-14,'test ring eigenvector N=11');

end

function errors = test_laplacian()
errors = 0;
N = 10;
G = gsp_path(N);

x = rand(N,1);

y1 = G.L*x;
y2 = -laplacianx_op(x);

if norm(y1-y2)/norm(y1)>10e-10;
   errors = errors +1;
   norm(y1-y2)/norm(y1)
   warning('OPERATOR: Error in laplacian test 1')

else
    fprintf('OPERATOR: laplacian 1 ok\n');
end

end

function errors = test_div()
errors = 0;
N = 10;
G = gsp_path(N);
G = gsp_adj2vec(G);

x = rand(N-1,1);

y1 = gsp_div(G,x);
y2 = -div_op1d([x;0]);

if norm(y1-y2)/norm(y1)>10e-10;
   errors = errors +1;
   norm(y1-y2)/norm(y1)
   warning('OPERATOR: Error in div test 1')

else
    fprintf('OPERATOR: div 1 ok\n');
end

end

function errors = test_div2()
errors = 0;
N = 10;
G = gsp_sensor(N);
G = gsp_adj2vec(G);

M = 5;

x = rand(N,M);

t = gsp_grad(G,x);

y1 = gsp_div(G,t);
y2 = gsp_div_old(G,t);

if norm(y1(:)-y2(:))/norm(y1(:))>10e-10;
   errors = errors +1;
   norm(y1(:)-y2(:))/norm(y1(:))
   warning('OPERATOR: Error in div test 2')

else
    fprintf('OPERATOR: div 2 ok\n');
end

end

function errors = test_div3()
errors = 0;
N = 10;
G = gsp_sensor(N);
G = gsp_adj2vec(G);

M = 5;

x = rand(N,M);

t = gsp_grad(G,x);

y1 = gsp_div(G,t);

y2 = zeros(N,M);
for ii = 1:M
    y2(:,ii) = gsp_div_old(G,t(:,ii));
end

if norm(y1(:)-y2(:))/norm(y1(:))>10e-10;
   errors = errors +1;
   norm(y1(:)-y2(:))/norm(y1(:))
   warning('OPERATOR: Error in div test 3')

else
    fprintf('OPERATOR: div 3 ok\n');
end

end

function errors = test_gradient()
errors = 0;
N = 50;
G = gsp_path(N);
G = gsp_adj2vec(G);

x = rand(N,1);

y1 = gsp_grad(G,x);
y2 = gradient_op1d(x);

if norm(y1-y2(1:(end-1)))/norm(y1)>10e-10;
   errors = errors +1;
   norm(y1-y2(1:(end-1)))/norm(y1)
   warning('OPERATOR: Error in gradient test 1')

else
    fprintf('OPERATOR: gradient 1 ok\n');
end

end

function errors = test_gradient2()
errors = 0;
N = 50;
G = gsp_sensor(N);
G = gsp_adj2vec(G);

M = 5;

x = rand(N,M);

y1 = gsp_grad(G,x);

y2 = zeros(length(G.v_in),M);
for ii = 1:M
    y2(:,ii) = gsp_grad(G,x(:,ii));
end

if norm(y1(:)-y2(:))/norm(y1(:))>10e-10;
   errors = errors +1;
   norm(y1(:)-y2(:))/norm(y1(:))
   warning('OPERATOR: Error in gradient test 2')

else
    fprintf('OPERATOR: gradient 2 ok\n');
end

end

function errors = test_gradient_mat()
errors = 0;
N = 50;
G = gsp_sensor(N);
G = gsp_adj2vec(G);

x = rand(N,1);

y1 = gsp_grad(G,x);

D = gsp_grad_mat(G);

y2 = D*x;

if norm(y1(:)-y2(:))/norm(y1(:))>10e-10;
   errors = errors +1;
   norm(y1(:)-y2(:))/norm(y1(:))
   warning('OPERATOR: Error in gradient mat test')

else
    fprintf('OPERATOR: gradient mat ok\n');
end

end

function errors = test_directed()
errors = 0;

try

    G = gsp_sensor(50);

    Gd = gsp_assign_rand_direction(G,.5);
    Gd = gsp_adj2vec(Gd);

    fprintf('OPERATOR: directed ok\n');
catch
    errors = errors + 1;
    warning('OPERATOR: Error directed test')
end

end

function errors = test_advection()
errors = 0;
G = gsp_sensor();

Gd = gsp_assign_rand_direction(G,.5);
Gd = gsp_adj2vec(Gd);

W = diag(diag(Gd.Adv)) - Gd.Adv';

err = norm(Gd.W-W,'fro')/norm(Gd.W,'fro');

if err>1e-10;
   errors = errors +1;
   err
   warning('OPERATOR: Error advection test')

else
    fprintf('OPERATOR: advection ok\n');
end

end

function errors = test_divgradient()
errors = 0;
N = 50;
G = gsp_sensor(N);
G = gsp_adj2vec(G);

x = rand(N,1);

y1 = gsp_div(G,gsp_grad(G,x));
y2 = G.L*x;

if norm(y1-y2)/norm(y1)>10e-10
   errors = errors +1;
   norm(y1-y2)/norm(y1)
   warning('OPERATOR: Error in divgradient test 1')

else
    fprintf('OPERATOR: divgradient 1 ok\n');
end

if norm(full(G.Diff'*G.Diff-G.L))>10e-10;
   errors = errors +1;
   norm(full(G.Diff'*G.Diff-G.L))
   warning('OPERATOR: Error in divgradient test 2')

else
    fprintf('OPERATOR: divgradient 2 ok\n');
end

G = gsp_create_laplacian(G,'normalized');
G = gsp_adj2vec(G);

y1 = gsp_div(G,gsp_grad(G,x));
y2 = G.L*x;

if norm(y1-y2)/norm(y1)>10e-10
   errors = errors +1;
   norm(y1-y2)/norm(y1)
   warning('OPERATOR: Error in divgradient test 3')

else
    fprintf('OPERATOR: divgradient 3 ok\n');
end

if norm(full(G.Diff'*G.Diff-G.L))>10e-10;
   errors = errors +1;
   norm(full(G.Diff'*G.Diff-G.L))
   warning('OPERATOR: Error in divgradient test 3')

else
    fprintf('OPERATOR: divgradient 3 ok\n');
end

end

function errors = test_fourier_basis_ring()
errors = 0;
N = 50;
G = gsp_ring(N);
G = gsp_compute_fourier_basis(G);

if sum(diff(G.e) < -1e-10)
   errors = errors +1;
   warning('OPERATOR: Error in the Fourier test 1')

else
    fprintf('OPERATOR: Fourier 1 ok\n');
end

if sum(norm(G.L-G.U*diag(G.e)*G.U','fro') < -1e-10)
   errors = errors +1;
   warning('OPERATOR: Error in the Fourier test 2')

else
    fprintf('OPERATOR: Fourier 2 ok\n');
end

N = 51;
G = gsp_ring(N);
G = gsp_compute_fourier_basis(G);

if sum(norm(G.L-G.U*diag(G.e)*G.U','fro') < -1e-10)
   errors = errors +1;
   warning('OPERATOR: Error in the Fourier test 2')

else
    fprintf('OPERATOR: Fourier 2 ok\n');
end

end

function errors = test_translate()
errors = 0;
N = 50;
k = 10;
G = gsp_ring(N);
G = gsp_compute_fourier_basis(G);

f = rand(N,1);

ft = gsp_translate(G,f,k);

if norm(ft-circshift(f,[-k+1,0]))<1e-10
    fprintf('OPERATOR: translate ok\n');
else
    errors = errors +1;
   warning('OPERATOR: Error in the translate test')

end

end

function errors = test_modulate()
errors = 0;
N = 30;
k = 10;
G = gsp_ring(N);
G = gsp_compute_fourier_basis(G);

f = rand(N,1);

% compute the ordering vectors
v = gsp_classic2graph_eig_order( N );
iv = 1:N;
iv(v) = iv;

ft = gsp_modulate(G,f,iv(k));

fthat(v) = gsp_gft(G,ft);
ft2hat(v) = gsp_gft(G,f); % back into the classical ordering
ft2hat = circshift(ft2hat,[0,-k+1]); % circshift

if norm(fthat-ft2hat)<1e-10
    fprintf('OPERATOR: modulate ok\n');
else
    errors = errors +1;
   warning('OPERATOR: Error in the modulate test')

end

end

function errors = test_kron_reduction()

errors = 0;

try
    figure(100)
      N = 64;
      param.distribute = 1;
      param.Nc = 5;
      param.regular = 1;
      G = gsp_sensor(N,param);
      ind = 1:2:N;
      Gnew = gsp_kron_reduce( G,ind );
      subplot(121)
      gsp_plot_graph(G);
      title('Original graph');
      subplot(122)
      gsp_plot_graph(Gnew);
      title('Kron reduction');

    close(100)

    fprintf('OPERATOR: KRON ok\n');
catch
    errors = errors +1;
   warning('OPERATOR: Error in the KRON test')

end

gsp_reset_seed(1)
      G1 = gsp_kron_reduce( G,ind );
      gsp_reset_seed(1)
      Lnew= gsp_kron_reduce_old( G.L,ind );
if norm(Lnew-G1.L,'fro')<1e-10
      fprintf('OPERATOR: KRON ok\n');
else
          errors = errors +1;
    norm(G2.W-G1.W,'fro')
   warning('OPERATOR: Error in the KRON test')
end

end

function errors = test_gft()

N = 30;
G = gsp_sensor(N);
G = gsp_compute_fourier_basis(G);

X = rand(N,20,10);

Xhat1 = gsp_gft(G,X);

Xhat2 = zeros(N,20,10);
for ii = 1:10
   Xhat2(:,:,ii) = gsp_gft(G, X(:,:,ii));
end

errors = gsp_assert_test(Xhat1,Xhat2,1e-10,'GFT');

end