x=zeros(2,100);
x(1,:)=linspace(-3,3,100);
x(2,:)=linspace(-3,3,100);

[X1,X2]=meshgrid(x(1,:),x(2,:));
Z=zeros(100,100);
for i=1:100
    for j=1:100
        Z(i,j)=obj2([X1(i,j),X2(i,j)]);
    end
end

plot3(X1,X2,Z)
% surf(X1,X2,Z)
% contour(X1,X2,Z)


function [ MRI_signal_noisy ] = add_Rician_noise_to_simulated_dMRI_data( MRI_signal, SNR )
%ADD_RICIAN_NOISE_TO_SIMULATED_DMRI_DATA adds Rician noise to simulated
%MRI_signals.
%
% inputs: noisefree MRI_signal and SNR
% outputs: noisy MRI_signal
%
% noisy signal, S = |S* + N1 + iN2|
% where S* = noise-free signal
% N1 and N2 are random numbers, drawn from normal distribution, with mean=0, and std = 1/SNR.

% Author: Joanne Bates <joanne.bates@eng.ox.ac.uk>
% Copyright (c) 2015 University of Oxford
%
% 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/>.

no_of_directions =  size(MRI_signal);
N1 = (1/SNR).*randn(no_of_directions);
N2 = (1/SNR).*randn(no_of_directions);

MRI_signal_noisy = abs(MRI_signal + N1 + 1i.*N2);
end


function yhat=spence21(y)
% function for Spencer's 21-point graduation rule.
% set out following Spencer's hand-calculation method,
% which isn't the shortest computer program!

oten = ones(10,1);
n = length(y);
y = [ oten*y(1); y; oten*y(n) ];

n = length(y);
k = 4:(n-3);
y1 = -y(k-3) + y(k-1) + 2*y(k) + y(k+1) -y(k+3);

n = length(y1);
k = 4:(n-3);
y2 = y1(k-3)+y1(k-2)+y1(k-1)+y1(k)+y1(k+1)+y1(k+2)+y1(k+3);

n = length(y2);
k = 3:(n-2);
y3 = y2(k-2)+y2(k-1)+y2(k)+y2(k+1)+y2(k+2);

n = length(y3);
k = 3:(n-2);
y4 = y3(k-2)+y3(k-1)+y3(k)+y3(k+1)+y3(k+2);

yhat = y4/350;
return;


clear all; close all;
I=imread('onion.png');
J=rgb2gray(I);
K=fft2(J);
L=fftshift(K);
M=ifft2(K);
figure;
subplot(121);
imshow(uint8(abs(L)/198));
subplot(122);
imshow(uint8(M));


function status = allEq( X, Y, THR )
% allEq() : rapidly assert X and Y are (almost exactly) the same
% Check if matrices X, Y are equal to within given tolerance
%   defined on the percent difference scale
%INPUT:
%   X, Y: N x D matrices
%   THR: numeric tolerance for equality (default 1e-8)
%OUTPUT
%  status : boolean
%     true if EVERY entry has perc diff <= THR
%     false o.w.

if ~exist( 'THR', 'var' )
    THR = 1e-8;
end

percDiff = abs( X - Y )./abs(X);
if max(percDiff) > THR
    status = false;
else
    status = true;
end


function [ a, seed ] = wathen_ge ( nx, ny, n, seed )

%*****************************************************************************80
%
%% WATHEN_GE returns the Wathen matrix, using general (GE) storage.
%
%  Discussion:
%
%    The Wathen matrix is a finite element matrix which is sparse.
%
%    The entries of the matrix depend in part on a physical quantity
%    related to density.  That density is here assigned random values between
%    0 and 100.
%
%    The matrix order N is determined by the input quantities NX and NY,
%    which would usually be the number of elements in the X and Y directions.
%    The value of N is
%
%      N = 3*NX*NY + 2*NX + 2*NY + 1,
%
%    The matrix is the consistent mass matrix for a regular NX by NY grid
%    of 8 node serendipity elements.
%
%    Here is an illustration for NX = 3, NY = 2:
%
%     23-24-25-26-27-28-29
%      |     |     |     |
%     19    20    21    22
%      |     |     |     |
%     12-13-14-15-16-17-18
%      |     |     |     |
%      8     9    10    11
%      |     |     |     |
%      1--2--3--4--5--6--7
%
%    For this example, the total number of nodes is, as expected,
%
%      N = 3 * 3 * 2 + 2 * 2 + 2 * 3 + 1 = 29
%
%    The matrix is symmetric positive definite for any positive values of the
%    density RHO(X,Y).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
%    Andrew Wathen,
%    Realistic eigenvalue bounds for the Galerkin mass matrix,
%    IMA Journal of Numerical Analysis,
%    Volume 7, Number 4, October 1987, pages 449-457.
%
%  Parameters:
%
%    Input, integer NX, NY, values which determine the size of the matrix.
%
%    Input, integer N, the number of variables.
%
%    Input/output, integer SEED, the random number seed.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  em = [
     6.0, -6.0,  2.0, -8.0,  3.0, -8.0,  2.0, -6.0;
    -6.0, 32.0, -6.0, 20.0, -8.0, 16.0, -8.0, 20.0;
     2.0, -6.0,  6.0, -6.0,  2.0, -8.0,  3.0, -8.0;
    -8.0, 20.0, -6.0, 32.0, -6.0, 20.0, -8.0, 16.0;
     3.0, -8.0,  2.0, -6.0,  6.0, -6.0,  2.0, -8.0;
    -8.0, 16.0, -8.0, 20.0, -6.0, 32.0, -6.0, 20.0;
     2.0, -8.0,  3.0, -8.0,  2.0, -6.0,  6.0, -6.0;
    -6.0, 20.0, -8.0, 16.0, -8.0, 20.0, -6.0, 32.0 ]';

  [ rho, seed ] = r8mat_uniform_01 ( nx, ny, seed );
  rho = 100.0 * rho;

  node = zeros(8,1);

  for j = 1 : ny

    for i = 1 : nx
%
%  For the element (I,J), determine the indices of the 8 nodes.
%
      node(1) = 3 * j * nx + 2 * j + 2 * i + 1;
      node(2) = node(1) - 1;
      node(3) = node(1) - 2;

      node(4) = ( 3 * j - 1 ) * nx + 2 * j + i - 1;
      node(8) = node(4) + 1;

      node(5) = ( 3 * j - 3 ) * nx + 2 * j + 2 * i - 3;
      node(6) = node(5) + 1;
      node(7) = node(5) + 2;

      for krow = 1 : 8
        for kcol = 1 : 8
          a(node(krow),node(kcol)) = a(node(krow),node(kcol)) ...
            + rho(i,j) * em(krow,kcol);
        end
      end

    end
  end

  return
end


function prob_test062 ( )

%*****************************************************************************80
%
%% TEST062 tests EXPONENTIAL_CDF, EXPONENTIAL_CDF_INV, EXPONENTIAL_PDF;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST062\n' );
  fprintf ( 1, '  For the Exponential CDF:\n' );
  fprintf ( 1, '  EXPONENTIAL_CDF evaluates the CDF.\n' );
  fprintf ( 1, '  EXPONENTIAL_CDF_INV inverts the CDF.\n' );
  fprintf ( 1, '  EXPONENTIAL_PDF evaluates the PDF.\n' );

  a = 1.0;
  b = 2.0;

  check = exponential_check ( a, b );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST062 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A = %14f\n', a );
  fprintf ( 1, '  PDF parameter B = %14f\n', b );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '       X            PDF           CDF            CDF_INV\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ x, seed  ] = exponential_sample ( a, b, seed );

    pdf = exponential_pdf ( x, a, b );

    cdf = exponential_cdf ( x, a, b );

    x2 = exponential_cdf_inv ( cdf, a, b );

    fprintf ( 1, ' %14f  %14f  %14f  %14f\n', x, pdf, cdf, x2 );

  end

  return
end


function H = hessian_2sided_nrows(f,x,k,varargin)
% Computes the last K rows of a 2-sided finite difference Hessian
%
% USAGE:
%   H=hessian_2sided_nrows(FUNC,X,K,VARARGIN)
%
% INPUTS:
%   FUNC         - Function name, fval = func(x,varargin)
%   X            - Vector of parameters (N x 1)
%   K            - Number of rows to compute
%   VARARGIN     - Optional arguments passed to the function
%
% OUTPUTS:
%   H            - Finite difference, 2-sided hessian: N x K
%
% COMMENTS:
%   Modification of hessian_2sided
%   See also HESSIAN_2SIDED

% Code originally from COMPECON toolbox [www4.ncsu.edu/~pfackler]
% documentation modified to fit the format of the Econometrics Toolbox
% by James P. LeSage, Dept of Economics
% University of Toledo
% 2801 W. Bancroft St,
% Toledo, OH 43606
% jlesage@spatial-econometrics.com
%
% Further modified (to do 2-sided numerical derivs, rather than 1) by:
% Modifications Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 9/1/2005

if size(x,2)>size(x,1)
    x=x';
end
if size(x,2)~=1
    error('X must be a column vector.')
end
n = size(x,1);

try
    feval(f,x,varargin{:});
catch FE
    errtxt=['There was an error evaluating the function.  Please check the arguements.  The specific error was:' FE.message];
    error(errtxt);
end

fx = feval(f,x,varargin{:});

% Compute the stepsize (h)
h = eps.^(1/3)*max(abs(x),1e-2);
xh = x+h;
h = xh-x;

ee = sparse(1:n,1:n,h,n,n);

% Compute forward and backward steps
gp = zeros(n,1);
gm = zeros(n,1);
for i=1:n
    gp(i) = feval(f,x+ee(:,i),varargin{:});
    gm(i) = feval(f,x-ee(:,i),varargin{:});
end

hh=h*h';
Hm=NaN*ones(n);
Hp=NaN*ones(n);
% Compute "double" forward and backward steps
for i=n-k+1:n
    for j=1:n
        Hp(i,j) = feval(f,x+ee(:,i)+ee(:,j),varargin{:});
        Hp(j,i)=Hp(i,j);
        Hm(i,j) = feval(f,x-ee(:,i)-ee(:,j),varargin{:});
        Hm(j,i)=Hm(i,j);
    end
end
%Compute the hessian
H = zeros(n);
for i=(n-k+1):n
    for j=1:n
        H(i,j) = (Hp(i,j)-gp(i)-gp(j)+fx+fx-gm(i)-gm(j)+Hm(i,j))/hh(i,j)/2;
        H(j,i) = H(i,j);
    end
end

%Only the final K rows
H=H((n-k+1):n,:);


function [ value, ifault ] = betain ( x, p, q, beta )

%*****************************************************************************80
%
%% BETAIN computes the incomplete Beta function ratio.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by KL Majumder, GP Bhattacharjee.
%    FORTRAN90 version by John Burkardt.
%
%  Reference:
%
%    KL Majumder, GP Bhattacharjee,
%    Algorithm AS 63:
%    The incomplete Beta Integral,
%    Applied Statistics,
%    Volume 22, Number 3, 1973, pages 409-411.
%
%  Parameters:
%
%    Input, real X, the argument, between 0 and 1.
%
%    Input, real P, Q, the parameters, which
%    must be positive.
%
%    Input, real BETA, the logarithm of the complete
%    beta function.
%
%    Output, real BETAIN, the value of the incomplete
%    Beta function ratio.
%
%    Output, integer IFAULT, error flag.
%    0, no error.
%    nonzero, an error occurred.
%
  acu = 0.1D-14;

  value = x;
  ifault = 0;
%
%  Check the input arguments.
%
  if ( p <= 0.0 | q <= 0.0 )
    ifault = 1;
    return
  end

  if ( x < 0.0 | 1.0 < x )
    ifault = 2;
    return
  end
%
%  Special cases.
%
  if ( x == 0.0 | x == 1.0 )
    return
  end
%
%  Change tail if necessary and determine S.
%
  psq = p + q;
  cx = 1.0 - x;

  if ( p < psq * x )
    xx = cx;
    cx = x;
    pp = q;
    qq = p;
    indx = 1;
  else
    xx = x;
    pp = p;
    qq = q;
    indx = 0;
  end

  term = 1.0;
  ai = 1.0;
  value = 1.0;
  ns = floor ( qq + cx * psq );
%
%  Use the Soper reduction formula.
%
  rx = xx / cx;
  temp = qq - ai;
  if ( ns == 0 )
    rx = xx;
  end

  while ( 1 )

    term = term * temp * rx / ( pp + ai );
    value = value + term;
    temp = abs ( term );

    if ( temp <= acu & temp <= acu * value )

      value = value * exp ( pp * log ( xx ) ...
      + ( qq - 1.0 ) * log ( cx ) - beta ) / pp;

      if ( indx )
        value = 1.0 - value;
      end

      break

    end

    ai = ai + 1.0;
    ns = ns - 1;

    if ( 0 <= ns )
      temp = qq - ai;
      if ( ns == 0 )
        rx = xx;
      end
    else
      temp = psq;
      psq = psq + 1.0;
    end

  end

  return
end


function value = f_02 ( x )

%*****************************************************************************80
%
%% F_02 evaluates 2*x-exp(-x).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the point at which F is to be evaluated.
%
%    Output, real VALUE, the value of the function at X.
%
  value = 2.0 * x - exp ( - x );

  return
end


function grid_index = multigrid_scale_closed ( dim_num, order_nd, level_max, ...
  level_1d, grid_index )

%*****************************************************************************80
%
%% MULTIGRID_SCALE_CLOSED renumbers a grid as a subgrid on a higher level.
%
%  Discussion:
%
%    This routine takes a grid associated with a given value of
%    LEVEL, and multiplies all the indices by a power of 2, so that
%    the indices reflect the position of the same points, but in
%    a grid of level LEVEL_MAX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer ORDER_ND, the number of points in the grid.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Input, integer LEVEL_1D(DIM_NUM), the level in each dimension.
%
%    Input, integer GRID_INDEX(DIM_NUM,POINT_NUM), the index
%    values for each grid point, based in the level for which the grid
%    was generated.
%
%    Output, integer GRID_INDEX(DIM_NUM,POINT_NUM), the index
%    values for each grid point, appropriate for the grid as a subgrid
%    of a grid of level LEVEL_MAX.
%
  for dim = 1 : dim_num

    if ( level_1d(dim) == 0 )

      if ( 0 == level_max )
        order_max = 1;
      else
        order_max = 2^level_max + 1;
      end

      grid_index(dim,1:order_nd) = floor ( ( order_max - 1 ) / 2 );

    else

      factor = 2^( level_max - level_1d(dim) );

      grid_index(dim,1:order_nd) = grid_index(dim,1:order_nd) * factor;

    end

  end

  return
end


function value = i4_width ( i )

%*****************************************************************************80
%
%% I4_WIDTH returns the "width" of an integer.
%
%  Example:
%
%        I  VALUE
%    -----  -------
%    -1234    5
%     -123    4
%      -12    3
%       -1    2
%        0    1
%        1    1
%       12    2
%      123    3
%     1234    4
%    12345    5
%
%  Discussion:
%
%    The width of an integer is the number of characters necessary to print it.
%
%    The width of an integer can be useful when setting the appropriate output
%    format for a vector or array of values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, the number whose width is desired.
%
%    Output, integer VALUE, the number of characters necessary to represent
%    the integer in base 10, including a negative sign if necessary.
%
  if ( 0 <= i )
    value = i4_log_10 ( i ) + 1;
  else
    value = i4_log_10 ( i ) + 2;
  end

  return
end


function spline_test07 ( )

%*****************************************************************************80
%
%% TEST07 tests BASIS_MATRIX_OVERHAUSER_UNI, BASIS_MATRIX_TMP.
%
%  Discussion:
%
%   YDATA(1:NDATA) = ( TDATA(1:NDATA) + 2 )**2 + 3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author
%
%    John Burkardt
%
  n = 4;
  ndata = 4;
  nsample = 4;
  tdata = [ -1.0E+00, 0.0E+00, 1.0E+00, 2.0E+00 ];
  ydata = [ 4.0E+00, 7.0E+00, 12.0E+00, 19.0E+00 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST07\n' );
  fprintf ( 1, '  BASIS_MATRIX_OVERHAUSER_UNI sets up the basis\n' );
  fprintf ( 1, '    matrix for the uniform Overhauser spline.\n' );

  mbasis = basis_matrix_overhauser_uni ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '    TDATA, YDATA\n' );
  fprintf ( 1, '\n' );
  for i = 1 : ndata
    fprintf ( 1, '%14f  %14f\n', tdata(i), ydata(i) );
  end

  left = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, '    T, Spline(T)\n' );
  fprintf ( 1, '\n' );

  for i = 0 : ndata

    if ( i == 0 )
      tlo = tdata(1) - 0.5E+00 * ( tdata(2) - tdata(1) );
      thi = tdata(1);
    elseif ( i < ndata )
      tlo = tdata(i);
      thi = tdata(i+1);
    elseif ( ndata <= i )
      tlo = tdata(ndata);
      thi = tdata(ndata) + 0.5E+00 * ( tdata(ndata) - tdata(ndata-1) );
    end

    if ( i < ndata )
      jhi = nsample - 1;
    else
      jhi = nsample;
    end

    for j = 0 : jhi

      tval = ( ( nsample - j ) * tlo   ...
             + (           j ) * thi ) ...
             /   nsample;

      yval = basis_matrix_tmp ( left, n, mbasis, ndata, tdata, ydata, tval );

      if ( 0 < i & j == 0 )
        mark = '*';
      else
        mark = ' ';
      end

      fprintf ( 1, '%c  %14f  %14f\n', mark, tval, yval );

    end

  end

  return
end


function [M2,NM2]=compute_Hist_Metric2(img1,img2,index)

img1 = double(img1)/255;
img2 = double(img2)/255;
img1 = img1 - 0.01*(img1==1);
img2 = img2 - 0.01*(img2==1);

N = length(index);
sN = 20000;
gray_level = 64;%round(N^(1/(2+alpha)));
s1 = img1(index);
s2 = img2(index);
rand('state',1);
% if sampling
%     sindex = round(rand(sN,1)*(N-1))+1;
%     s1 = s1(sindex);
%     s2 = s2(sindex);
%     gray_level = round(sN^(1/(2+alpha)));
% end

bitLen = floor(log2(gray_level)-0.01) + 1;

sd1 = floor(s1*gray_level);
sd2 = floor(s2*gray_level);

h1 = getHist(sd1,bitLen);
h2 = getHist(sd2,bitLen);

h12 = getHist([sd1 sd2],bitLen);

e1 = -sum(h1.*log(h1));
e2 = -sum(h2.*log(h2));

e12 = -sum(h12.*log(h12));

M2 = e12 - e1- e2;
NM2 = M2/e12;


function mse = calc_mse(V, W, H)

    [m, n] = size(V);

    f_val = nmf_cost(V, W, H, []);

    mse = 2 * f_val / (m*n);

end


clear;
clc;
format short;

%% 线性代数及其应用第5版 5.8 Iterative Estimates For EienValues

%% EXAMPLE 1
A = [1.8 0.8; 0.2 1.2];
v1 = [4 1]';
x = [-0.5 1]';

for i = 1:8
    x = A*x;
end

x = x / norm(x);
v1= v1 / norm(v1);
%
% fprintf("可以看到 x 逼近v1的方向\r\n");
% x
% v1

%% EXAMPLE 2
A  = [6 5; 1 2];
x = [0 1]';

for i = 1:8
    x = A*x;
    lanbda = abs(max(x));
    x  = x / lanbda;

end

x
lanbda


% Studies the recovery probability of different algorithms
% at a fixed sparsity level and varying number of signals.

close all;
clear all;
clc;
rng('default');
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');
% Signal space
N = 256;
% Number of measurements
M = 48;
% Number of signals
Ss = 1:32;
% Sparsity level
K = 16;
num_trials = 100;
num_ss = length(Ss);
success_with_s.somp = zeros(num_ss, 1);
success_with_s.ra_omp = zeros(num_ss, 1);
success_with_s.ra_ormp = zeros(num_ss, 1);
success_with_s.gomp_2_mmv = zeros(num_ss, 1);
success_with_s.gomp_4_mmv = zeros(num_ss, 1);

snr_threshold = 100;

for ns=1:num_ss
    % Current number of signals
    S = Ss(ns);
    num_successes.somp = 0;
    num_successes.ra_omp = 0;
    num_successes.ra_ormp = 0;
    num_successes.gomp_2_mmv = 0;
    num_successes.gomp_4_mmv = 0;
    for nt=1:num_trials
        % Sensing matrix
        Phi = spx.dict.simple.gaussian_dict(M, N);
        % Sparse signal generator
        gen  = spx.data.synthetic.SparseSignalGenerator(N, K, S);
        % Gaussian distributed non-zero samples
        X = gen.gaussian;
        % Measurement vectors
        Y = Phi * X;

        % Create the solver for simultaneous orthogonal matching pursuit
        solver = spx.pursuit.joint.OrthogonalMatchingPursuit(Phi, K, 2);
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.somp = num_successes.somp + success;

        % Create the solver for rank aware orthogonal matching pursuit
        solver = spx.pursuit.joint.RankAwareOMP(Phi, K);
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.ra_omp = num_successes.ra_omp + success;

        % Create the solver for rank aware order recursive matching pursuit
        solver = spx.pursuit.joint.RankAwareORMP(Phi, K);
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.ra_ormp = num_successes.ra_ormp + success;

        % Create the solver for Generalized OMP MMV with L=2
        solver = spx.pursuit.joint.GOMP(Phi, K, 2);
        solver.L = 2;
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.gomp_2_mmv = num_successes.gomp_2_mmv + success;

        % Create the solver for Generalized OMP MMV with L=4
        solver = spx.pursuit.joint.GOMP(Phi, K, 2);
        solver.L = 4;
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.gomp_4_mmv = num_successes.gomp_4_mmv + success;

        fprintf('S: %d, K=%d, trial=%d\n', S, K, nt);
    end
    success_with_s.somp(ns) = num_successes.somp / num_trials;
    success_with_s.ra_omp(ns) = num_successes.ra_omp / num_trials;
    success_with_s.ra_ormp(ns) = num_successes.ra_ormp / num_trials;
    success_with_s.gomp_2_mmv(ns) = num_successes.gomp_2_mmv / num_trials;
    success_with_s.gomp_4_mmv(ns) = num_successes.gomp_4_mmv / num_trials;
end

save ('bin/success_with_s_comparison.mat');


% This script generates randomly several testing
% Quadratic Programming tasks.
%
% The generated tasks are stored to ../data/ folder.
%

randn('state',0);
rand('state',0);

%=======================================================
% Quadratic Programming task with simplex constraints
% solved by LIBQP_SPLX
%=======================================================

% TASK 1: splxcon_qp_random100x10
% number of variables 100
% number of equlity and inequality constraints 10 (not considering constraints x >= 0)
I = [];
for i=1:10;
  I = [I i*ones(1,10)];
end
Z = randn(length(I),length(I)*2); H=Z*Z';
f=randn(length(I),1);
b = rand(10,1)*10;

% 5 randomly selected constraints will be equlities and inequlities
S = zeros(10,1);
idx = randperm(10);
S(idx(1:5)) = 1;

save ../data/splx_qp_random100x10 H f b I S;

% TASK 2: splxcon_qp_random300x30
% number of variables 300
% number of equlity and inequality constraints 30 (not considering constraints x >= 0)
I = [];
for i=1:30;
  I = [I i*ones(1,10)];
end
Z = randn(length(I),length(I)*2); H=Z*Z';
f=randn(length(I),1);
b = rand(30,1)*10;

% 5 randomly selected constraints will be equlity and inequlity
S = zeros(30,1);
idx = randperm(30);
S(idx(1:15)) = 1;

save ../data/splx_qp_random300x30 H f b I S;

%=======================================================
% Quadratic Programming task solved by GSMO algorithm
% solved by LIBQP_GSMO
%=======================================================

% TASK 1: gsmo_qp_random100
% number of variables 100

f=randn(100,1)*20;
Z = randn(length(f),length(f)*2); H=Z*Z';

a = randn(1,length(f)); a(find(a==0)) = 1;
b = randn(1);
UB = rand(length(f),1);
LB = zeros(length(f),1);

save ../data/gsmo_qp_random100 H f a b UB LB;

% TASK 2: gsmo_qp_random300
% number of variables 300

f=randn(300,1)*20;
Z = randn(length(f),length(f)*2); H=Z*Z';

a = randn(1,length(f)); a(find(a==0)) = 1;
b = randn(1);
UB = zeros(length(f),1);
LB = -rand(length(f),1);

save ../data/gsmo_qp_random300 H f a b UB LB;


function gegenbauer_rule ( order, alpha, a, b, filename )

%*****************************************************************************80
%
%% GEGENBAUER_RULE generates a Gauss-Gegenbauer rule.
%
%  Discussion:
%
%    This program computes a standard Gauss-Gegenbauer quadrature rule
%    and writes it to a file.
%
%    The user specifies:
%    * the ORDER (number of points) in the rule;
%    * the ALPHA parameter;
%    * A, the left endpoint;
%    * B, the right endpoint;
%    * FILENAME, the root name of the output files.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2010
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GEGENBAUER_RULE\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compute a Gauss-Gegenbauer rule for approximating\n' );
  fprintf ( 1, '    Integral ( A <= x <= B ) ((x-A)(B-X))^ALPHA f(x) dx\n' );
  fprintf ( 1, '  of order ORDER.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The user specifies ORDER, ALPHA, A, B, and FILENAME.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ORDER is the number of points:\n' );
  fprintf ( 1, '  ALPHA is the exponent:\n' );
  fprintf ( 1, '  A is the left endpoint;\n' );
  fprintf ( 1, '  B is the right endpoint;\n' );
  fprintf ( 1, '  FILENAME is used to generate 3 files:\n' );
  fprintf ( 1, '    filename_w.txt - the weight file\n' );
  fprintf ( 1, '    filename_x.txt - the abscissa file.\n' );
  fprintf ( 1, '    filename_r.txt - the region file.\n' );
%
%  Initialize the parameters.
%
  beta = 0.0;
%
%  Get ORDER.
%
  if ( nargin < 1 )
    order = input ( '  Enter the rule order ORDER:  ' );
  elseif ( ischar ( order ) )
    order = str2num ( order );
  else
%
%  Get ALPHA.
%
  if ( nargin < 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  ALPHA is the exponent of ((x-A)(B-X)) in the integral:\n' );
    fprintf ( 1, '  Note that -1.0 < ALPHA is required.\n' );
    fprintf ( 1, '\n' );
    alpha = input ( '  Enter the value of ALPHA:  ' );
  elseif ( ischar ( alpha ) )
    alpha = str2num ( alpha );
  end
%
%  Get A.
%
  if ( nargin < 3 )
    a = input ( '  Enter the left endpoint A:  ' );
  elseif ( ischar ( a ) )
    a = str2num ( a );
  end
%
%  Get B.
%
  if ( nargin < 4 )
    b = input ( '  Enter the right endpoint B:  ' );
  elseif ( ischar ( b ) )
    b = str2num ( b );
  end
%
%  Get FILENAME.
%
  if ( nargin < 5 )
    fprintf ( 1,  '\n' );
    fprintf ( 1,  '  FILENAME is the ''root name'' of the quadrature files).\n' );
    filename = input ( '  Enter FILENAME as a quoted string:  ' );
  end
%
%  Input summary.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ORDER = %d\n', order );
  fprintf ( 1, '  ALPHA = %f\n', alpha );
  fprintf ( 1, '  A = %f\n', a );
  fprintf ( 1, '  B = %f\n', b );
  fprintf ( 1, '  FILENAME = "%s".\n', filename );
%
%  Construct the rule.
%
  kind = 3;
  [ x, w ] = cgqf ( order, kind, alpha, beta, a, b );
%
%  Write the rule.
%
  r = [ a, b ]';
  rule_write ( order, filename, x, w, r );
%
%  Terminate.
%
  fprintf ( 1,  '\n' );
  fprintf ( 1,  'GEGENBAUER_RULE:\n' );
  fprintf ( 1,  '  Normal end of execution.\n' );
  fprintf ( 1,  '\n' );
  timestamp ( );

  return
end
function [ t, wts ] = cdgqf ( nt, kind, alpha, beta )

%*****************************************************************************80
%
%% CDGQF computes a Gauss quadrature formula with default A, B and simple knots.
%
%  Discussion:
%
%    This routine computes all the knots and weights of a Gauss quadrature
%    formula with a classical weight function with default values for A and B,
%    and only simple knots.
%
%    There are no moments checks and no printing is done.
%
%    Use routine EIQFS to evaluate a quadrature computed by CGQFS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer NT, the number of knots.
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev,            (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,inf)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-inf,inf)  |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,inf)     (x-a)^alpha*(x+b)^beta
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Output, real T(NT), the knots.
%
%    Output, real WTS(NT), the weights.
%
  parchk ( kind, 2 * nt, alpha, beta );
%
%  Get the Jacobi matrix and zero-th moment.
%
  [ aj, bj, zemu ] = class_matrix ( kind, nt, alpha, beta );
%
%  Compute the knots and weights.
%
  [ t, wts ] = sgqf ( nt, aj, bj, zemu );

  return
end
function [ t, wts ] = cgqf ( nt, kind, alpha, beta, a, b )

%*****************************************************************************80
%
%% CGQF computes knots and weights of a Gauss quadrature formula.
%
%  Discussion:
%
%    The user may specify the interval (A,B).
%
%    Only simple knots are produced.
%
%    The user may request that the routine print the knots and weights,
%    and perform a moment check.
%
%    Use routine EIQFS to evaluate this quadrature formula.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer NT, the number of knots.
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev Type 1,     (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,+oo)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-oo,+oo)   |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,+oo)     (x-a)^alpha*(x+b)^beta
%    9, Chebyshev Type 2,     (a,b)       ((b-x)*(x-a))^(+0.5)
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Input, real A, B, the interval endpoints.
%
%    Output, real T(NT), the knots.
%
%    Output, real WTS(NT), the weights.
%

%
%  Compute the Gauss quadrature formula for default values of A and B.
%
  [ t, wts ] = cdgqf ( nt, kind, alpha, beta );
%
%  All knots have multiplicity = 1.
%
  mlt = zeros(nt,1);
  mlt(1:nt) = 1;
%
%  NDX(I) = I.
%
  ndx = ( 1 : nt );
%
%  Scale the quadrature rule.
%
  [ t, wts ] = scqf ( nt, t, mlt, wts, nt, ndx, kind, alpha, beta, a, b );

  return
end
function [ aj, bj, zemu ] = class_matrix ( kind, m, alpha, beta )

%*****************************************************************************80
%
%% CLASS_MATRIX computes the Jacobi matrix for a quadrature rule.
%
%  Discussion:
%
%    This routine computes the diagonal AJ and subdiagonal BJ
%    elements of the order M tridiagonal symmetric Jacobi matrix
%    associated with the polynomials orthogonal with respect to
%    the weight function specified by KIND.
%
%    For weight functions 1-7, M elements are defined in BJ even
%    though only M-1 are needed.  For weight function 8, BJ(M) is
%    set to zero.
%
%    The zero-th moment of the weight function is returned in ZEMU.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev,            (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,inf)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-inf,inf)  |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,inf)     (x-a)^alpha*(x+b)^beta
%
%    Input, integer M, the order of the Jacobi matrix.
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Output, real AJ(M), BJ(M), the diagonal and subdiagonal
%    of the Jacobi matrix.
%
%    Output, real ZEMU, the zero-th moment.
%
  temp = eps;

  parchk ( kind, 2 * m - 1, alpha, beta );

  temp2 = 0.5;

  if ( 500.0 * temp < abs ( ( gamma ( temp2 ) )^2 - pi ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CLASS - Fatal error!\n' );
    fprintf ( 1, '  Gamma function does not match machine parameters.\n' );
    error ( 'CLASS - Fatal error!' );
  end

  bj = zeros(m,1);
  aj = zeros(m,1);

  if ( kind == 1 )

    ab = 0.0;

    zemu = 2.0 / ( ab + 1.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      abi = i + ab * mod ( i, 2 );
      abj = 2 * i + ab;
      bj(i) = abi * abi / ( abj * abj - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 2 )

    zemu = pi;

    aj(1:m) = 0.0;

    bj(1) =  sqrt ( 0.5 );
    bj(2:m) = 0.5;

  elseif ( kind == 3 )

    ab = alpha * 2.0;
    zemu = 2.0^( ab + 1.0 ) * gamma ( alpha + 1.0 )^2 ...
      / gamma ( ab + 2.0 );

    aj(1:m) = 0.0;
    bj(1) = 1.0 / ( 2.0 * alpha + 3.0 );
    for i = 2 : m
      bj(i) = i * ( i + ab ) / ( 4.0 * ( i + alpha )^2 - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 4 )

    ab = alpha + beta;
    abi = 2.0 + ab;
    zemu = 2.0^( ab + 1.0 ) * gamma ( alpha + 1.0 ) ...
      * gamma ( beta + 1.0 ) / gamma ( abi );
    aj(1) = ( beta - alpha ) / abi;
    bj(1) = 4.0 * ( 1.0 + alpha ) * ( 1.0 + beta ) ...
      / ( ( abi + 1.0 ) * abi * abi );
    a2b2 = beta * beta - alpha * alpha;

    for i = 2 : m
      abi = 2.0 * i + ab;
      aj(i) = a2b2 / ( ( abi - 2.0 ) * abi );
      abi = abi^2;
      bj(i) = 4.0 * i * ( i + alpha ) * ( i + beta ) * ( i + ab ) ...
        / ( ( abi - 1.0 ) * abi );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 5 )

    zemu = gamma ( alpha + 1.0 );

    for i = 1 : m
      aj(i) = 2.0 * i - 1.0 + alpha;
      bj(i) = i * ( i + alpha );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 6 )

    zemu = gamma ( ( alpha + 1.0 ) / 2.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      bj(i) = ( i + alpha * mod ( i, 2 ) ) / 2.0;
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 7 )

    ab = alpha;
    zemu = 2.0 / ( ab + 1.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      abi = i + ab * mod(i,2);
      abj = 2 * i + ab;
      bj(i) = abi * abi / ( abj * abj - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 8 )

    ab = alpha + beta;
    zemu = gamma ( alpha + 1.0 ) * gamma ( - ( ab + 1.0 ) ) ...
      / gamma ( - beta );
    apone = alpha + 1.0;
    aba = ab * apone;
    aj(1) = - apone / ( ab + 2.0 );
    bj(1) = - aj(1) * ( beta + 1.0 ) / ( ab + 2.0 ) / ( ab + 3.0 );
    for i = 2 : m
      abti = ab + 2.0 * i;
      aj(i) = aba + 2.0 * ( ab + i ) * ( i - 1 );
      aj(i) = - aj(i) / abti / ( abti - 2.0 );
    end

    for i = 2 : m - 1
      abti = ab + 2.0 * i;
      bj(i) = i * ( alpha + i ) / ( abti - 1.0 ) * ( beta + i ) ...
        / ( abti^2 ) * ( ab + i ) / ( abti + 1.0 );
    end

    bj(m) = 0.0;
    bj(1:m) =  sqrt ( bj(1:m) );

  end

  return
end
function [ d, z ] = imtqlx ( n, d, e, z )

%*****************************************************************************80
%
%% IMTQLX diagonalizes a symmetric tridiagonal matrix.
%
%  Discussion:
%
%    This routine is a slightly modified version of the EISPACK routine to
%    perform the implicit QL algorithm on a symmetric tridiagonal matrix.
%
%    The authors thank the authors of EISPACK for permission to use this
%    routine.
%
%    It has been modified to produce the product Q' * Z, where Z is an input
%    vector and Q is the orthogonal matrix diagonalizing the input matrix.
%    The changes consist (essentialy) of applying the orthogonal transformations
%    directly to Z as they are generated.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%    Roger Martin, James Wilkinson,
%    The Implicit QL Algorithm,
%    Numerische Mathematik,
%    Volume 12, Number 5, December 1968, pages 377-383.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real D(N), the diagonal entries of the matrix.
%
%    Input, real E(N), the subdiagonal entries of the
%    matrix, in entries E(1) through E(N-1).
%
%    Input, real Z(N), a vector to be operated on.
%
%    Output, real D(N), the diagonal entries of the diagonalized matrix.
%
%    Output, real Z(N), the value of Q' * Z, where Q is the matrix that
%    diagonalizes the input symmetric tridiagonal matrix.
%
  itn = 30;

  prec = eps;

  if ( n == 1 )
    return
  end

  e(n) = 0.0;

  for l = 1 : n

    j = 0;

    while ( 1 )

      for m = l : n

        if ( m == n )
          break
        end

        if ( abs ( e(m) ) <= prec * ( abs ( d(m) ) + abs ( d(m+1) ) ) )
          break
        end

      end

      p = d(l);

      if ( m == l )
        break
      end

      if ( j == itn )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'IMTQLX - Fatal error!\n' );
        fprintf ( 1, '  Iteration limit exceeded.\n' );
        error ( 'IMTQLX - Fatal error!' );
      end

      j = j + 1;
      g = ( d(l+1) - p ) / ( 2.0 * e(l) );
      r =  sqrt ( g * g + 1.0 );
      g = d(m) - p + e(l) / ( g + r8_sign ( g ) * abs ( r ) );
      s = 1.0;
      c = 1.0;
      p = 0.0;
      mml = m - l;

      for ii = 1 : mml

        i = m - ii;
        f = s * e(i);
        b = c * e(i);

        if ( abs ( f ) >= abs ( g ) )
          c = g / f;
          r =  sqrt ( c * c + 1.0 );
          e(i+1) = f * r;
          s = 1.0 / r;
          c = c * s;
        else
          s = f / g;
          r =  sqrt ( s * s + 1.0 );
          e(i+1) = g * r;
          c = 1.0 / r;
          s = s * c;
        end

        g = d(i+1) - p;
        r = ( d(i) - g ) * s + 2.0 * c * b;
        p = s * r;
        d(i+1) = g + p;
        g = c * r - b;
        f = z(i+1);
        z(i+1) = s * z(i) + c * f;
        z(i) = c * z(i) - s * f;

      end

      d(l) = d(l) - p;
      e(l) = g;
      e(m) = 0.0;

    end

  end

  for ii = 2 : n

     i = ii - 1;
     k = i;
     p = d(i);

     for j = ii : n
       if ( d(j) < p )
         k = j;
         p = d(j);
       end
     end

     if ( k ~= i )
       d(k) = d(i);
       d(i) = p;
       p = z(i);
       z(i) = z(k);
       z(k) = p;
     end

  end

  return
end
function parchk ( kind, m, alpha, beta )

%*****************************************************************************80
%
%% PARCHK checks parameters ALPHA and BETA for classical weight functions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev,            (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,inf)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-inf,inf)  |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,inf)     (x-a)^alpha*(x+b)^beta
%
%    Input, integer M, the order of the highest moment to
%    be calculated.  This value is only needed when KIND = 8.
%
%    Input, real ALPHA, BETA, the parameters, if required
%    by the value of KIND.
%
  if ( kind <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PARCHK - Fatal error!\n' );
    fprintf ( 1, '  KIND <= 0.\n' );
    error ( 'PARCHK - Fatal error!' );
  end
%
%  Check ALPHA for Gegenbauer, Jacobi, Laguerre, Hermite, Exponential.
%
  if ( 3 <= kind && alpha <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PARCHK - Fatal error!\n' );
    fprintf ( 1, '  3 <= KIND and ALPHA <= -1.\n' );
    error ( 'PARCHK - Fatal error!' );
  end
%
%  Check BETA for Jacobi.
%
  if ( kind == 4 && beta <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PARCHK - Fatal error!\n' );
    fprintf ( 1, '  KIND == 4 and BETA <= -1.0.\n' );
    error ( 'PARCHK - Fatal error!' );
  end
%
%  Check ALPHA and BETA for rational.
%
  if ( kind == 8 )
    tmp = alpha + beta + m + 1.0;
    if ( 0.0 <= tmp || tmp <= beta )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'PARCHK - Fatal error!\n' );
      fprintf ( 1, '  KIND == 8 but condition on ALPHA and BETA fails.\n' );
      error ( 'PARCHK - Fatal error!' );
    end
  end

  return
end
function value = r8_sign ( x )

%*****************************************************************************80
%
%% R8_SIGN returns the sign of an R8.
%
%  Discussion:
%
%    The value is +1 if the number is positive or zero, and it is -1 otherwise.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the number whose sign is desired.
%
%    Output, real VALUE, the sign of X.
%
  if ( 0 <= x )
    value = +1.0;
  else
    value = -1.0;
  end

  return
end
function r8mat_write ( output_filename, m, n, table )

%*****************************************************************************80
%
%% R8MAT_WRITE writes an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string OUTPUT_FILENAME, the output filename.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real TABLE(M,N), the points.
%

%
%  Open the file.
%
  output_unit = fopen ( output_filename, 'wt' );

  if ( output_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_WRITE - Error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'R8MAT_WRITE - Error!' );
  end
%
%  Write the data.
%
%  For smaller data files, and less precision, try:
%
%     fprintf ( output_unit, '  %14.6f', table(i,j) );
%
  for j = 1 : n
    for i = 1 : m
      fprintf ( output_unit, '  %24.16f', table(i,j) );
    end
    fprintf ( output_unit, '\n' );
  end
%
%  Close the file.
%
  fclose ( output_unit );

  return
end
function rule_write ( order, filename, x, w, r )

%*****************************************************************************80
%
%% RULE_WRITE writes a quadrature rule to a file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ORDER, the order of the rule.
%
%    Input, string FILENAME, specifies the output files.
%    write files 'filename_w.txt', 'filename_x.txt', 'filename_r.txt' defining
%    weights, abscissas, and region.
%
%    Input, real X(ORDER), the abscissas.
%
%    Input, real W(ORDER), the weights.
%
%    Input, real R(2), the region.
%
  filename_x = strcat ( filename, '_x.txt' );
  filename_w = strcat ( filename, '_w.txt' );
  filename_r = strcat ( filename, '_r.txt' );

  fprintf ( 1, '\n' );
  fprintf ( 1,'  Creating quadrature files.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  "Root" file name is   "%s".\n', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Weight file will be   "%s".\n', filename_w );
  fprintf ( 1, '  Abscissa file will be "%s".\n', filename_x );
  fprintf ( 1, '  Region file will be   "%s".\n', filename_r );

  r8mat_write ( filename_w, 1, order, w' );
  r8mat_write ( filename_x, 1, order, x' );
  r8mat_write ( filename_r, 1, 2,     r' );

  return
end
function [ t, wts ] = scqf ( nt, t, mlt, wts, nwts, ndx, kind, alpha, ...
  beta, a, b )

%*****************************************************************************80
%
%% SCQF scales a quadrature formula to a nonstandard interval.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer NT, the number of knots.
%
%    Input, real T(NT), the original knots.
%
%    Input, integer MLT(NT), the multiplicity of the knots.
%
%    Input, real WTS(NWTS), the weights.
%
%    Input, integer NWTS, the number of weights.
%
%    Input, integer NDX(NT), used to index the array WTS.
%    For more details see the comments in CAWIQ.
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev Type 1,     (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,+oo)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-oo,+oo)   |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,+oo)     (x-a)^alpha*(x+b)^beta
%    9, Chebyshev Type 2,     (a,b)       ((b-x)*(x-a))^(+0.5)
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Input, real A, B, the interval endpoints.
%
%    Output, real T(NT), the scaled knots.
%
%    Output, real WTS(NWTS), the scaled weights.
%
  temp = eps;

  parchk ( kind, 1, alpha, beta )

  if ( kind == 1 )

    al = 0.0;
    be = 0.0;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  elseif ( kind == 2 )

    al = -0.5;
    be = -0.5;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  elseif ( kind == 3 )

    al = alpha;
    be = alpha;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  elseif ( kind == 4 )

    al = alpha;
    be = beta;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  elseif ( kind == 5 )

    if ( b <= 0.0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  B <= 0.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = a;
    slp = 1.0 / b;
    al = alpha;
    be = 0.0;

  elseif ( kind == 6 )

    if ( b <= 0.0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  B <= 0.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = a;
    slp = 1.0 / sqrt ( b );
    al = alpha;
    be = 0.0;

  elseif ( kind == 7 )

    al = alpha;
    be = 0.0;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  elseif ( kind == 8 )

    if ( a + b <= 0.0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  A + B <= 0.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = a;
    slp = a + b;
    al = alpha;
    be = beta;

  elseif ( kind == 9 )

    al = 0.5;
    be = 0.5;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  end

  p = slp^( al + be + 1.0 );

  for k = 1 : nt

    t(k) = shft + slp * t(k);
    l = abs ( ndx(k) );

    if ( l ~= 0 )
      tmp = p;
      for i = l : l + mlt(k) - 1
        wts(i) = wts(i) * tmp;
        tmp = tmp * slp;
      end
    end

  end

  return
end
function [ t, wts ] = sgqf ( nt, aj, bj, zemu )

%*****************************************************************************80
%
%% SGQF computes knots and weights of a Gauss Quadrature formula.
%
%  Discussion:
%
%    This routine computes all the knots and weights of a Gauss quadrature
%    formula with simple knots from the Jacobi matrix and the zero-th
%    moment of the weight function, using the Golub-Welsch technique.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer NT, the number of knots.
%
%    Input, real AJ(NT), the diagonal of the Jacobi matrix.
%
%    Input, real BJ(NT), the subdiagonal of the Jacobi
%    matrix, in entries 1 through NT-1.  On output, BJ has been overwritten.
%
%    Input, real ZEMU, the zero-th moment of the weight function.
%
%    Output, real T(NT), the knots.
%
%    Output, real WTS(NT), the weights.
%

%
%  Exit if the zero-th moment is not positive.
%
  if ( zemu <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SGQF - Fatal error!\n' );
    fprintf ( 1, '  ZEMU <= 0.\n' );
    error ( 'SGQF - Fatal error!' );
  end
%
%  Set up vectors for IMTQLX.
%
  wts = zeros ( nt, 1 );

  wts(1) = sqrt ( zemu );
  wts(2:nt) = 0.0;
%
%  Diagonalize the Jacobi matrix.
%
  [ t, wts ] = imtqlx ( nt, aj, bj, wts );

  wts(1:nt) = wts(1:nt).^2;

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


function value = r8_factorial ( n )

%*****************************************************************************80
%
%% R8_FACTORIAL returns N!.
%
%  Definition:
%
%    N! = Product ( 1 <= I <= N ) I
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 1999
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the argument of the function.
%    0 <= N.
%
%    Output, real VALUE, the factorial of N.
%
  if ( n < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_FACTORIAL - Fatal error!\n' );
    fprintf ( 1, '  N < 0.\n' );
    error ( 'R8_FACTORIAL - Fatal error!' );
  end

  value = 1.0;

  for i = 2 : n
    value = value * i;
  end

  return
end


%% grid2patch
% Below is a demonstration of the features of the |grid2patch| function

%%
clear; close all; clc;

%% Syntax
% |[F,V,C]=grid2patch(X,Y,Z,C,perdiocOpt);|

%% Description
% This function generates patch data for gridded surface coordinates
% defined by X,Y,Z (e.g. as produced by functions like meshgrid or
% ndgrid). Optional inputs include the color data C (a color for each
% vertex), perdiocOpt (a 1x2 logic) which sets periodicity of the surface
% in the first or second direction.
% Similar to MATLAB's |surf2patch| function (which does not include
% periodic connectivity).

%% Examples

%%
% Plot settings
cMap=gjet(250);
fontSize=15;

%% Example: Converting gridded surface data to patch data
[X,Y]=ndgrid(linspace(0,2*pi,25));
Z=sin(X)+sin(Y);

%%
% Creating patch data for the cylinger
[F,V,C]=grid2patch(X,Y,Z,Z);

%%
% Visualizing outputs

cFigure; hold on;
gpatch(F,V,C,'k');
axisGeom(gca,fontSize); camlight headlight;
drawnow;

%% Example: periodicity in direction 1

n=12;
t=linspace(-pi,pi,n+1);
t=t(1:end-1);
v1=[cos(t(:)) sin(t(:)) zeros(size(t(:))) ];
v2=3*v1;

%Get coordinate matrices
m=5;
X=linspacen(v1(:,1),v2(:,1),m);
Y=linspacen(v1(:,2),v2(:,2),m);
Z=linspacen(v1(:,3),v2(:,3),m);

%Collect node set
VN=[X(:) Y(:) Z(:)];
numSteps=n-1;

%%
% Creating patch data for the cylinger
[F1,V1]=grid2patch(X,Y,Z,[],[0 0]); %Not periodic
[F2,V2]=grid2patch(X,Y,Z,[],[1 0]); %Periodic in first direction

%%
% Visualizing outputs

cFigure;
subplot(1,2,1); hold on;
title('Not periodic')
gpatch(F1,V1,'bw','k');
gpatch(patchBoundary(F1),V1,'none','r',1,3);
axisGeom(gca,fontSize); camlight headlight;

subplot(1,2,2); hold on;
title('Periodic in 1st direction')
gpatch(F2,V2,'bw','k');
gpatch(patchBoundary(F2),V2,'none','r',1,3);
axisGeom(gca,fontSize); camlight headlight;
drawnow;

%% Example: periodicity in direction 2

%%
% Creating an example of a cylinder
t=linspace(-pi,pi,25);
t=t(1:end-1);
[T,Z]=meshgrid(t,linspace(0,1,5));

[X,Y]=pol2cart(T,ones(size(T)));

%%
% Creating patch data for the cylinger
[F1,V1,C1]=grid2patch(X,Y,Z,T,[0 0]); %Not periodic
[F2,V2,C2]=grid2patch(X,Y,Z,T,[0 1]); %Periodic in first direction

%%
% Visualizing outputs

cFigure;
subplot(1,2,1); hold on;
title('Not periodic')
gpatch(F1,V1,C1,'k');
gpatch(patchBoundary(F1),V1,'none','r',1,3);
axisGeom(gca,fontSize); camlight headlight;

subplot(1,2,2); hold on;
title('Periodic in 2nd direction')
gpatch(F2,V2,C2,'k');
gpatch(patchBoundary(F2),V2,'none','r',1,3);
axisGeom(gca,fontSize); camlight headlight;
drawnow;

%% Example: periodicity in both directions

%%
% Creating an example of a cylinder
n=12; m=25;
t=linspace(-pi,pi,n+1);
t=t(1:end-1);
v=[cos(t(:)) zeros(size(t(:))) sin(t(:))];
v(:,1)=v(:,1)+3;

a=linspace(0,2*pi,m+1); a=a(1:end-1);
X=zeros(n,m);
Y=zeros(n,m);
Z=zeros(n,m);

for q=1:1:m
    R=euler2DCM([0 0 a(q)]);
    vr=v*R;
   X(:,q)=vr(:,1);
   Y(:,q)=vr(:,2);
   Z(:,q)=vr(:,3);
end

%%
% Creating patch data for the cylinger
[F1,V1,C1]=grid2patch(X,Y,Z,Z,[0 0]); %Not periodic
[F2,V2,C2]=grid2patch(X,Y,Z,Z,[0 1]); %Periodic in first direction
[F3,V3,C3]=grid2patch(X,Y,Z,Z,[1 0]); %Periodic in second direction
[F4,V4,C4]=grid2patch(X,Y,Z,Z,[1 1]); %Periodic both directions

%%
% Visualizing outputs

cFigure;
subplot(2,2,1); hold on;
title('Not periodic')
gpatch(F1,V1,C1,'k');
gpatch(patchBoundary(F1),V1,'none','r',1,3);
axisGeom(gca,fontSize); camlight headlight;

subplot(2,2,2); hold on;
title('Periodic in 1st direction')
gpatch(F2,V2,C2,'k');
gpatch(patchBoundary(F2),V2,'none','r',1,3);
axisGeom(gca,fontSize); camlight headlight;

subplot(2,2,3); hold on;
title('Periodic in 2nd direction')
gpatch(F3,V3,C3,'k');
gpatch(patchBoundary(F3),V3,'none','r',1,3);
axisGeom(gca,fontSize); camlight headlight;

subplot(2,2,4); hold on;
title('Periodic in 1st and 2nd direction')
gpatch(F4,V4,C4,'k');
gpatch(patchBoundary(F4),V4,'none','r',1,3);
axisGeom(gca,fontSize); camlight headlight;

drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*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 geometry_test207 ( )

%*****************************************************************************80
%
%% TEST207 tests TRIANGLE_CONTAINS_POINT_2D_1, TRIANGLE_CONTAINS_POINT_2D_2, TRIANGLE_CONTAINS_POINT_2D_3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 June 2006
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;
  test_num = 7;

  p_test = [ ...
     0.25,   0.25; ...
     0.75,   0.25; ...
     1.00,   1.00; ...
    11.00,   0.50; ...
     0.00,   1.00; ...
     0.50, -10.00; ...
     0.60,   0.60 ]';
  t = [ ...
    0.0, 1.0; ...
    0.0, 0.0; ...
    1.0, 0.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST207\n' );
  fprintf ( 1, '  For a triangle in 2D,\n' );
  fprintf ( 1, '  TRIANGLE_CONTAINS_POINT_2D_1 reports if a point \n' );
  fprintf ( 1, '    is inside a triangle (and doesn''t care about\n' );
  fprintf ( 1, '    the ordering of the vertices)\n' );
  fprintf ( 1, '  TRIANGLE_CONTAINS_POINT_2D_2 reports if a point \n' );
  fprintf ( 1, '    is inside a triangle (and DOES care about\n' );
  fprintf ( 1, '    the ordering of the vertices)\n' );
  fprintf ( 1, '  TRIANGLE_CONTAINS_POINT_2D_3 reports if a point \n' );
  fprintf ( 1, '    is inside a triangle (and doesn''t care about\n' );
  fprintf ( 1, '    the ordering of the vertices)\n' );

  r8mat_transpose_print ( dim_num, 3, t, '  Triangle vertices:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     X       Y     In1  In2  In3\n' );
  fprintf ( 1, '\n' );

  for j = 1 : test_num

    p(1:dim_num) = p_test(1:dim_num,j);

    inside1 = triangle_contains_point_2d_1 ( t, p );
    inside2 = triangle_contains_point_2d_2 ( t, p );
    inside3 = triangle_contains_point_2d_3 ( t, p );

    fprintf ( 1, '  %10f  %10f  %1d  %1d  %1d\n', ...
      p(1:dim_num), inside1, inside2, inside3 );

  end
%
%  Make a copy of the triangle with vertices in reverse order.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Repeat the test, but reverse the triangle vertex\n' );
  fprintf ( 1, '  ordering.\n' );

  for j = 1 : 3
    t2(1:2,j) = t(1:2,4-j);
  end

  r8mat_transpose_print ( dim_num, 3, t2, '  Triangle vertices (reversed):' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     X       Y     In1  In2  In3\n' );
  fprintf ( 1, '\n' );

  for j = 1 : test_num

    p(1:dim_num) = p_test(1:dim_num,j);

    inside1 = triangle_contains_point_2d_1 ( t2, p );
    inside2 = triangle_contains_point_2d_2 ( t2, p );
    inside3 = triangle_contains_point_2d_3 ( t2, p );

    fprintf ( 1, '  %10f  %10f  %1d  %1d  %1d\n', ...
      p(1:dim_num), inside1, inside2, inside3 );

  end

  return
end


%---------------------------------------------------------
% Low-pass FIR digital differentiator (LPFIRDD) design   -
% via constrained quadratic programming (QP)             -
% [Copt,c]=lpfirdd(N,alpha,beta,r,idraw)                 -
% By Dr Yangquan Chen		019-07-1999                   -
% Email=<yqchen@ieee.org>; URL=http://www.crosswinds.net/~yqchen/
% --------------------------------------------------------
% LPFIRDD: only 1st order derivative estimate
% total taps=2N. c(i)=-c(i+N+1); c(N+1)=0 (central point)
%
%          -N      -N+1            -1                    N
% FIR=c(1)z   +c(2)z    +...+ c(N)z  + 0 + ... + c(2N+1)z
%
%       N
%     ------
%     \                    j   -j
%      >       Copt(j) * (z - z  )
%     /
%     ------
%      j=1
% N: Taps  (N=2, similar to sgfilter(2,2,1,1)
% alpha ~ beta: transit band of frequency
%				    (in percentage of Nyquest freq)
% r: the polynomial order. r<=N Normally, set it to 1.
%---------------------------------------------------------------
function [Copt,bd]=lpfirdd(N,alpha,beta,r,idraw)
% testing parameters
% alpha=1./pi;beta=1.5/pi;N=10;r=1;idraw=1;
if (alpha>beta)
   disp('Error in alpha (alpha<=beta)');return;
end
if ((beta>1) | (beta <0))
   disp('Error in Beta! (beta in [0,1]');return;
end
if ((alpha>1) | (alpha <0))
   disp('Error in Alpha! (Alpha in [0,1]');return;
end
% default r=1
if (r<1); r=1; end
% matrix W
W=zeros(r,N);
for ix=1:N;for jx=1:r;
      W(jx,ix)=ix^(2*jx-1);
end;end
%matrix L
L=zeros(N,1);
if (beta>alpha)
	for ix=1:N
	   L(ix)=(alpha*sin(ix*beta*pi)-beta*sin(ix*alpha*pi))/ix/ix/(beta-alpha);
	end
elseif (beta==alpha)
	for ix=1:N
	   L(ix)=(ix*alpha*pi*cos(ix*alpha*pi)-sin(ix*alpha*pi))/ix/ix;
	end
end
% matrix e
ex=zeros(r,1);ex(1)=1;
% optimal solution
Copt=W'*inv(W*W')*(ex + 2.*W*L/pi)-2.*L/pi
Copt=Copt/2;
% fr plots
if (idraw==1)
   bd=[-fliplr(Copt'),0,Copt']';
	%ad=1;sys_sg=tf(bd',ad,1./Fs);bode(sys_sg)
	Fs=12790;nL=N;nR=N;npts=1000;%w=logspace(0,4,npts);
	w=((1:npts)-1)*pi/npts;
	j=sqrt(-1);ejw=zeros(nL+nR+1,npts);
	for ix=(-nL:nR)
		ejw(ix+nL+1,:)=exp(j*ix*w);
	end
	freq=bd'*ejw;
	figure;subplot(2,1,1)
	plot(w/pi*Fs/2,(abs(freq)));grid on;
	hold on; ax=axis;ax(2)=Fs/2;axis(ax);
	xlabel('freq. (Hz)');ylabel('amplitude (dB)');
	subplot(2,1,2)
	plot(w/pi*Fs/2,180*(angle(freq))/pi );grid on;
	hold on; ax=axis;ax(2)=Fs/2;axis(ax);
   xlabel('freq. (Hz)');ylabel('phase anlge (deg.)');

	figure;subplot(2,1,1);Fs=12600; % Hz for U8
	semilogx(w*Fs/pi/2,20*log10(abs(freq)));grid on;
	hold on; ax=axis;ax(2)=Fs/2;axis(ax);
	semilogx([Fs/2,Fs/2],[ax(3),ax(4)],'o-r');grid on;
	xlabel('freq. (Hz)');ylabel('amplitude (dB)');
	subplot(2,1,2)
	semilogx(w*Fs/pi/2,180*(angle(freq))/pi );grid on;
	hold on; ax=axis;ax(2)=Fs/2;axis(ax);
	semilogx([Fs/2,Fs/2],[ax(3),ax(4)],'o-r');grid on;
	xlabel('freq. (Hz)');ylabel('phase anlge (deg.)');
end
return


function [vFrequency, vAmplitude] = fastfft(vData, SampleRate, Plot)

%FASTFFT   Create useful data from an FFT operation.
%   Usage: [vFrequency, vAmplitude] = fastfft(vData, SampleRate, [Plot])
%
%   (no plot will be shown if the last input == 0 or is not included)
%
%   This function inputs 'vData' as a vector (row or column),
%   'SampleRate' as a number (samples/sec), 'Plot' as anything,
%   and does the following:
%
%     1: Removes the DC offset of the data
%     2: Puts the data through a hanning window
%     3: Calculates the Fast Fourier Transform (FFT)
%     4: Calculates the amplitude from the FFT
%     5: Calculates the frequency scale
%     6: Optionally creates a Bode plot
%
%   Created 7/22/03, Rick Auch, mekaneck@campbellsville.com

%Make vData a row vector
if size(vData,2)==1
    vData = vData';
end

%Calculate number of data points in data
n = length(vData);

%Remove DC Offset
vData = vData - mean(vData);

%Put data through hanning window using hanning subfunction
vData = hanning(vData);

%Calculate FFT
vData = fft(vData);

%Calculate amplitude from FFT (multply by sqrt(8/3) because of effects of hanning window)
vAmplitude = abs(vData)*sqrt(8/3);

%Calculate frequency scale
vFrequency = linspace(0,n-1,n)*(SampleRate/n);

%Limit both output vectors due to Nyquist criterion
DataLimit = ceil(n/2);
vAmplitude = vAmplitude(1:DataLimit);
vFrequency = vFrequency(1:DataLimit);

if exist('Plot', 'var')==1 & Plot~=0
    plot(vFrequency, vAmplitude);
    title('Bode Plot');
    xlabel('Frequency (Hz)');
    ylabel('Amplitude');
end

%------------------------------------------------------------------------------------------
%Hanning Subfunction
function vOutput = hanning(vInput)
% This function takes a vector input and outputs the same vector,
% multiplied by the hanning window function

%Determine the number of input data points
n = length(vInput);

%Initialize the vector
vHanningFunc = linspace(0,n-1,n);

%Calculate the hanning funtion
vHanningFunc = .5*(1-cos(2*pi*vHanningFunc/(n-1)));

%Output the result
vOutput = vInput.*vHanningFunc;


function binv = beta_inv (x, a, b)
%  BETA_INV Inverse of the beta cumulative distribution function (cdf).
%     X = BETA_INV(P,A,B) returns the inverse of the beta cdf with
%     parameters A and B at the values in P.
%
% Copyright (c) 1995-1997, 2005-2007 Kurt Hornik

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

if (nargin ~= 3)
  error('must provide 3 parameters')
end

if (~isscalar (a) || ~isscalar(b))
  if ((size(a,1) ~= size(b,1)) || (size(b,1) ~= size(x,1)))
    error ('betainv: x, a and b must be of common size or scalars');
  end
end

[n,nin] = size(x);
binv = zeros(n,nin);

k = find ((x<0) || (x>1) || (a<0) || (b<0) || isnan(x));
if (any(k))
  binv(k) = NaN;
end

k = find ((x==1) && (a>0) && (b>0));
if (any (k))
  binv(k) = 1;
end

k = find ((x > 0) & (x < 1) & (a > 0) & (b > 0));
if (any (k))
  if (~isscalar(a) || ~isscalar(b))
    a = a(k);
    b = b(k);
    y = a./(a+b);
  else
    y = a/(a + b)*ones(size(k));
  end
  x = x(k);
  l = find (y < eps);
  if (any (l))
    y(l) = sqrt (eps) * ones (length (l), 1);
  end
  l = find (y > 1 - eps);
  if (any (l))
    y(l) = 1 - sqrt (eps) * ones (length (l), 1);
  end

  y_old = y;
  for i = 1 : 10000
    h     = (beta_cdf (y_old, a, b) - x) ./ beta_pdf (y_old, a, b);
    y_new = y_old - h;
    ind   = find (y_new <= eps);
    if (any (ind))
      y_new (ind) = y_old (ind) / 10;
    end
    ind = find (y_new >= 1 - eps);
    if (any (ind))
      y_new (ind) = 1 - (1 - y_old (ind)) / 10;
    end
    h = y_old - y_new;
    if (max (abs (h)) < sqrt (eps))
      break;
    end
    y_old = y_new;
  end

  binv(k) = y_new;
end

end


function varargout = fisher(varargin)
% VL_FISHER    Fisher vector feature encoding
%   ENC = VL_FISHER(X, MEANS, COVARIANCES, PRIORS) computes the Fisher
%   vector encoding of the vectors X relative to the Gaussian mixture
%   model with means MEANS, covariances COVARIANCES, and prior mode
%   probabilities PRIORS.
%
%   X has one column per data vector (e.g. a SIFT descriptor), and
%   MEANS and COVARIANCES one column per GMM component (covariance
%   matrices are assumed diagonal, hence these are simply the variance
%   of each data dimension). PRIORS has size equal to the number of
%   GMM components. All data must be of the same class, either SINGLE
%   or DOUBLE.
%
%   ENC is a vector of the same class of X of size equal to the
%   product of the data dimension and the number of components.
%
%   By default, the standard Fisher vector is computed. VL_FISHER()
%   accepts the following options:
%
%   Normalized::
%     If specified, L2 normalize the Fisher vector.
%
%   SquareRoot::
%     If specified, the signed square root function is applied to
%     ENC before normalization.
%
%   Improved::
%     If specified, compute the improved variant of the Fisher
%     Vector. This is equivalent to specifying the Normalized and
%     SquareRoot options.
%
%   Fast::
%     If specified, uses slightly less accurate computations but
%     significantly increase the speed in some cases (particularly
%     with a large number of Gaussian modes).
%
%   Verbose::
%     Increase the verbosity level (may be specified multiple times).
%
%   See: Fisher vectors, VL_HELP().
[varargout{1:nargout}] = vl_fisher(varargin{:});


function [P loglikelihood] = LearnCPDsGivenGraph(dataset, G, labels)
%
% Inputs:
% dataset: N x 10 x 3, N poses represented by 10 parts in (y, x, alpha)
% G: graph parameterization as explained in PA description
% labels: N x 2 true class labels for the examples. labels(i,j)=1 if the
%         the ith example belongs to class j and 0 elsewhere
%
% Outputs:
% P: struct array parameters (explained in PA description)
% loglikelihood: log-likelihood of the data (scalar)
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

N = size(dataset, 1);
K = size(labels,2);

loglikelihood = 0;
P.c = zeros(1,K);

% estimate parameters
% fill in P.c, MLE for class probabilities
% fill in P.clg for each body part and each class
% choose the right parameterization based on G(i,1)
% compute the likelihood - you may want to use ComputeLogLikelihood.m
% you just implemented.
%%%%%%%%%%%%%%%%%%%%%%%%%
% YOUR CODE HERE
if size(size(G),2) == 2
	G1 = G; G2 = G;
else
	G1 = reshape(G(:,:,1),10,2);
	G2 = reshape(G(:,:,2),10,2);
end
human = dataset([labels(:,1)==1],:,:);
alien = dataset([labels(:,2)==1],:,:);
P.c = [size(human,1),size(alien,1)];
P.c /= N;
for i = 1:10
	if G1(i,1) == 0
		[P.clg(i).mu_y(1),P.clg(i).sigma_y(1)] = FitGaussianParameters(human(:,i,1));
		[P.clg(i).mu_x(1),P.clg(i).sigma_x(1)] = FitGaussianParameters(human(:,i,2));
		[P.clg(i).mu_angle(1),P.clg(i).sigma_angle(1)] = FitGaussianParameters(human(:,i,3));
	else
		% calc beta and sigma
		[Betay,P.clg(i).sigma_y(1)] = FitLinearGaussianParameters(human(:,i,1),reshape(human(:,G1(i,2),:),size(human,1),3));
		[Betax,P.clg(i).sigma_x(1)] = FitLinearGaussianParameters(human(:,i,2),reshape(human(:,G1(i,2),:),size(human,1),3));
		[Betaangle,P.clg(i).sigma_angle(1)] = FitLinearGaussianParameters(human(:,i,3),reshape(human(:,G1(i,2),:),size(human,1),3));
		P.clg(i).theta(1,:) = [Betay(4),Betay(1:3)',Betax(4),Betax(1:3)',Betaangle(4),Betaangle(1:3)'];

	end
	if G2(i,1) == 0
		[P.clg(i).mu_y(2),P.clg(i).sigma_y(2)] = FitGaussianParameters(alien(:,i,1));
		[P.clg(i).mu_x(2),P.clg(i).sigma_x(2)] = FitGaussianParameters(alien(:,i,2));
		[P.clg(i).mu_angle(2),P.clg(i).sigma_angle(2)] = FitGaussianParameters(alien(:,i,3));
	else
		%calc beta and sigma
		[Betay,P.clg(i).sigma_y(2)] = FitLinearGaussianParameters(alien(:,i,1),reshape(alien(:,G2(i,2),:),size(alien,1),3));
		[Betax,P.clg(i).sigma_x(2)] = FitLinearGaussianParameters(alien(:,i,2),reshape(alien(:,G2(i,2),:),size(alien,1),3));
		[Betaangle,P.clg(i).sigma_angle(2)] = FitLinearGaussianParameters(alien(:,i,3),reshape(alien(:,G2(i,2),:),size(alien,1),3));
		P.clg(i).theta(2,:) = [Betay(4),Betay(1:3)',Betax(4),Betax(1:3)',Betaangle(4),Betaangle(1:3)'];
	end
end
loglikelihood = ComputeLogLikelihood(P,G,dataset);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('log likelihood: %f\n', loglikelihood);


function [P, R] = mdp_example_rand (S, A, is_sparse, mask)

% mdp_example_rand    Generate a random Markov Decision Process
% Arguments -------------------------------------------------------------
%   S = number of states (> 0)
%   A = number of actions (> 0)
%   is_sparse = false to have matrices in plain format, true to have sparse matrices
%               optional (default false).
%   mask(SxS) = matrix with 0 and 1 (0 indicates a place for a zero probability),
%               optional (default, ones(S,S) )
% Evaluation -------------------------------------------------------------
%   P(SxSxA) = transition probability matrix
%   R(SxSxA) = reward matrix

% MDPtoolbox: Markov Decision Processes Toolbox
% Copyright (C) 2009  INRA
% Redistribution and use in source and binary forms, with or without modification,
% are permitted provided that the following conditions are met:
%    * Redistributions of source code must retain the above copyright notice,
%      this list of conditions and the following disclaimer.
%    * Redistributions in binary form must reproduce the above copyright notice,
%      this list of conditions and the following disclaimer in the documentation
%      and/or other materials provided with the distribution.
%    * Neither the name of the <ORGANIZATION> nor the names of its contributors
%      may be used to endorse or promote products derived from this software
%      without specific prior written permission.
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
% ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
% IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
% INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
% BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
% DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
% OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
% OF THE POSSIBILITY OF SUCH DAMAGE.

% arguments checking
if S < 1 || A < 1
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: The number of states S ')
    disp('and the number of actions A must be upper than 1.')
    disp('--------------------------------------------------------')
elseif nargin >= 4 && ( size(mask,1) ~= S || size(mask,2) ~= S )
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: mask must be a SxS matrix')
    disp('--------------------------------------------------------')
else

    % initialization of optional arguments
    if nargin < 3; is_sparse = false; end;
    if nargin < 4; mask = ones(S,S); end;

    if is_sparse
        % definition of transition matrix : square stochastic matrix
        P = {};
        for a=1:A
            PP = sparse(mask .* rand(S));
            for s=1:S
                PP(s,:) = PP(s,:) / sum( PP(s,:) );
            end;
            P{a} = PP;
        end;

        % definition of reward matrix (values between -1 and +1)
        R = {};
        for a=1:A
	    R{a} = sparse(mask .* ( 2*rand(S) - ones(S,S) ));
        end;
    else
        % definition of transition matrix : square stochastic matrix
        for a=1:A
            P(:,:,a) = mask .* rand(S);
            for s=1:S
                P(s,:,a) = P(s,:,a) / sum( P(s,:,a) );
            end;
        end;

        % definition of reward matrix (values between -1 and +1)
        for a=1:A
	    R(:,:,a) = mask .* ( 2*rand(S) - ones(S,S) );
        end;
    end

end;


function [F] = Filter1D(N,Nc,s)

% function [F] = Filter1D(N,Nc,s)
% Purpose : Initialize 1D filter matrix of size N.
%           Order of exponential filter is (even) s with cutoff at Nc;

Globals1D;
filterdiag = ones(N+1,1);
alpha = -log(eps);

% Initialize filter function
for i=Nc:N
    filterdiag(i+1) = exp(-alpha*((i-Nc)/(N-Nc))^s);
end;

F = V*diag(filterdiag)*invV;
return;


function L = gaussianLogLikelihood(noise, mu, varsigma, y)

% GAUSSIANLOGLIKELIHOOD Log-likelihood of data under Gaussian noise model.

% NOISE

N = size(mu, 1);
D = size(mu, 2);
varsigma = varsigma + noise.sigma2;
for i = 1:D
  mu(:, i) = mu(:, i) + noise.bias(i);
end
arg = (y - mu);
arg = arg.*arg./varsigma;

L = - 0.5*sum(sum(log(varsigma))) ...
    - 0.5*sum(sum(arg)) ...
    - 0.5*N*D*log(2*pi);


% ASTROTIK by Francesco Santilli
% f2v computes velocity vector of an orbit at different true anomalies
%
% Usage: V = f2v(orbit,f,mi)
%
% where: orbit = [p e i o w] = 3d orbit elements
%        orbit = [p e w]     = 2d orbit elements
%           p = semi-latus rectum [L] (p>0)
%           e = eccentricity [-] (e>=0)
%           i = inclinaion [rad]
%           o = raan [rad]
%           w = argument of perifocus [rad]
%        f(k) = true anomalies [rad]
%        mi = gravitational parameter [L^3*T^-2] (mi>0)
%        V(k,:) = velocity [L*T^-1]

function V = f2v(orbit,f,mi)

    if ~(nargin == 3)
        error('Wrong number of input arguments.')
    end

    D = check(orbit,1);
    K = check(f,1);
    check(mi,0)

    if ~(D==3 || D==5)
        error('Wrong size of input arguments.')
    end

    d3 = (D==5);
    f = f(:);
    cf = cos(f);
    sf = sin(f);

    % rotation matrix
    if d3
        [p,e,i,o,w] = take(orbit);
        R = rotation([o i w]);
        R = R(1:2,:);
    else
        [p,e,w] = take(orbit);
        cw = cos(w);
        sw = sin(w);
        R = [ cw sw
             -sw cw];
    end

    if mi <= 0
        error('mi must be a strictly positive value.')
    end

    if p <= 0
        error('p must be a stricly positive value.')
    end

    if e < 0
        error('e must be a positive value.')
    end

    % velocity
    v = sqrt(mi/p);
    V = v * [-sf e+cf] * R;

end


function ex9mbvp
%EX9MBVP  Example 9 of the BVP tutorial, solved as a multi-point BVP
%   This boundary value problem is the subject of Chapter 8 of
%   C.C. Lin and L.A. Segel, Mathematics Applied to Deterministic
%   Problems in the Natural Sciences, SIAM, Philadelphia, 1988.
%   The ODEs
%
%      v' = (C - 1)/n
%      C' = (vC - min(x,1))/eta
%
%   are solved on the interval [0, lambda]. The boundary conditions
%   are v(0) = 0, C(lambda) = 1, and continuity of v(x) and C(x) at
%   x = 1.  Example EX9BVP shows how this three-point BVP is
%   reformulated for solution with BVP4C present in MATLAB 6.0.
%   Starting with MATLAB 7.0, BVP4C solves multi-point BVPs directly.
%
%   The quantity of most interest is the emergent osmolarity Os =
%   1/v(lambda).  The parameters are related to another parameter
%   kappa by eta = lambda^2/(n*kappa^2).  Lin and Segel develop an
%   approximate solution for Os valid for "small" n.  Here the BVP is
%   solved for a range of kappa when lambda = 2 and n = 0.005.  The
%   computed Os is compared to the approximation of Lin and Segel.

% Copyright 2004, The MathWorks, Inc.

  % Known parameters, visible in nested functions.
  n = 5e-2;
  lambda = 2;

  % Initial mesh - duplicate the interface point x = 1.
  xinit = [0, 0.25, 0.5, 0.75, 1, 1, 1.25, 1.5, 1.75, 2];   lambda = 2;

  sol = bvpinit(xinit,[1 1]);

  fprintf(' kappa    computed Os  approximate Os \n')
  for kappa = 2:5
    eta = lambda^2/(n*kappa^2);
    % After creating function handles, the new value of eta
    % will be used in nested functions.
    sol = bvp4c(@ex9mode,@ex9mbc,sol);

    K2 = lambda*sinh(kappa/lambda)/(kappa*cosh(kappa));
    approx = 1/(1 - K2);
    computed = 1/sol.y(1,end);
    fprintf('  %2i    %10.3f    %10.3f \n',kappa,computed,approx);
  end

  figure
  plot(sol.x,sol.y(1,:),sol.x,sol.y(2,:),'--')
  legend('v(x)','C(x)')
  title('A three-point BVP.')
  xlabel(['\lambda = ',num2str(lambda),', \kappa = ',num2str(kappa),'.'])
  ylabel('v and C')

  % --------------------------------------------------------------------------
  % Nested functions
  %

  function dydx = ex9mode(x,y,region)
  %EX9MODE  ODE function for Example 9 of the BVP tutorial.
  % Here the problem is solved directly, as a three-point BVP.
    dydx = zeros(2,1);
    dydx(1) = (y(2) - 1)/n;
    % The definition of C'(x) depends on the region.
    switch region
      case 1    % x in [0 1]
        dydx(2) = (y(1)*y(2) - x)/eta;
      case 2    % x in [1 lambda]
        dydx(2) = (y(1)*y(2) - 1)/eta;
    end
  end % ex9mode

  % --------------------------------------------------------------------------

  function res = ex9mbc(YL,YR)
  %EX9MBC  Boundary conditions for Example 9 of the BVP tutorial.
  % Here the problem is solved directly, as a three-point BVP.
    res = [ YL(1,1)             % v(0) = 0
            YR(1,1) - YL(1,2)   % continuity of v(x) at x = 1
            YR(2,1) - YL(2,2)   % continuity of C(x) at x = 1
            YR(2,end) - 1    ]; % C(lambda) = 1
  end % ex9mbc

  % --------------------------------------------------------------------------

end  % ex9mbvp


function [ value, ifault ] = gammds ( x, p )

%*****************************************************************************80
%
%% GAMMDS computes the incomplete Gamma integral.
%
%  Discussion:
%
%    The parameters must be positive.  An infinite series is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by Chi Leung Lau.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Chi Leung Lau,
%    Algorithm AS 147:
%    A Simple Series for the Incomplete Gamma Integral,
%    Applied Statistics,
%    Volume 29, Number 1, 1980, pages 113-114.
%
%  Parameters:
%
%    Input, real X, P, the arguments of the incomplete
%    Gamma integral.  X and P must be greater than 0.
%
%    Output, real VALUE, the value of the incomplete
%    Gamma integral.
%
%    Output, integer IFAULT, error flag.
%    0, no errors.
%    1, X <= 0 or P <= 0.
%    2, underflow during the computation.
%
  e = 1.0E-09;
  uflo = 1.0E-37;
%
%  Check the input.
%
  if ( x <= 0.0 )
    ifault = 1;
    value = 0.0;
    return
  end

  if ( p <= 0.0 )
    ifault = 1;
    value = 0.0;
    return
  end
%
%  ALNGAM is the natural logarithm of the gamma function.
%
  arg = p * log ( x ) - alngam ( p + 1.0 ) - x;

  if ( arg < log ( uflo ) )
    value = 0.0;
    ifault = 2;
    return
  end

  f = exp ( arg );

  if ( f == 0.0 )
    value = 0.0;
    ifault = 2;
    return
  end

  ifault = 0;
%
%  Series begins.
%
  c = 1.0;
  value = 1.0;
  a = p;

  while ( 1 )

    a = a + 1.0;
    c = c * x / a;
    value = value + c;

    if ( c <= e * value )
      break;
    end

  end

  value = value * f;

  return
end


%% Examples 10.17, 10.26, and 10.38: Benes-Daum and EKF/ERTS examples
%
% Copyright:
%   2018 - Simo Särkkä and Arno Solin
%
% License:
%   This software is provided under the MIT License. See the accompanying
%   LICENSE file for details.

%%
% Benes and Benes-Daum filter tests
%
    % Check that we have the correct normalization constants and
    % moments. We should have (for x = x(t) with t >= t_k)
    %
    % p(x | Y_k) =
    % 1/sqrt{2 pi P} exp(-(x-m)^2/(2P)) exp(-1/2 P) cosh(x) / cosh(m)
    %
    % and
    %
    %    E[x] = m + tanh(m) P
    %  E[x^2] = m^2 + P^2 + 2 m tanh(m) P + P
    %    V[x] = P + [1 - tanh(m)^2] P^2

    dx = 0.001;
    xx = -15:dx:15;
    dens = @(x,m,P) 1/sqrt(2*pi*P)*exp(-P/2)/cosh(m) * exp(-(x-m).^2/(2*P)) .* cosh(x);

    m = 1;
    P = 5;
    pp = dens(xx,m,P);
    figure(1); clf; plot(xx,pp);

    sum(pp .* dx)
    [sum(xx .* pp * dx)        m+tanh(m)*P]
    [sum(xx.^2 .* pp * dx)  m^2+P^2+2*m*tanh(m)*P+P]
    [sum((xx-m-tanh(m)*P).^2 .* pp * dx) P+(1-tanh(m)^2)*P^2]

%%
% Generate data
%
  % Lock random seed
  if exist('rng') % Octave doesn't have rng
    rng(1)
  else
    randn('state',9);
  end

    steps = 500;
    meas_mod = 20;
    dt = 0.01;
    R = 0.1;

    X = [];
    Y = [];
    T = [];
    x0 = 0;
    x = x0;
    t = 0;
    for k=1:steps
        x = x + tanh(x) * dt + sqrt(dt)*randn;
        if rem(k,meas_mod) == 0
            y = x + sqrt(R)*randn;
        else
            y = NaN;
        end
        t = t + dt;
        X(k) = x;
        Y(k) = y;
        T(k) = t;
    end

    figure(1); clf
    plot(T,X,T,Y,'o');

%%
% Benes-Daum filter
%
    EB = zeros(1,length(Y));
    VB = zeros(1,length(Y));
    MMB = zeros(1,length(Y));
    PPB = zeros(1,length(Y));

    mb0 = 0.1;
    Pb0 = 1;

    mb = mb0;
    Pb = Pb0;

    m0 = mb + Pb * tanh(mb);
    P0 = Pb + (1-tanh(mb)^2) * Pb^2;

    for k=1:length(Y)
        %
        % The Benes-Daum filter
        %
        Pb = Pb + dt;
        if ~isnan(Y(k))
            mb = mb + Pb / (R + Pb)*(Y(k) - mb);
            Pb = Pb - Pb^2/(R + Pb);
        end
        MMB(:,k) = mb;
        PPB(:,k) = Pb;
        EB(:,k) = mb + Pb * tanh(mb);
        VB(:,k) = Pb + (1-tanh(mb)^2) * Pb^2;
    end

    figure(1); clf;
    subplot(2,1,1);
      plot(T,MMB,T,EB);
    subplot(2,1,2);
      plot(T,PPB,T,VB);

    rmse_b = sqrt(mean((EB - X).^2))

%%
% Determine the quantiles of Benes solution
%
    QQ1 = zeros(size(MMB));
    QQ2 = zeros(size(MMB));
    for k=1:length(MMB)
        pp = dens(xx,MMB(:,k),PPB(:,k));
        cu = cumsum(pp ./ sum(pp));
        ind1 = find(cu >= 2.5/100,1);
        ind2 = find(cu >= 97.5/100,1);
        QQ1(k) = xx(ind1);
        QQ2(k) = xx(ind2);
    end

    figure(1); clf;
    plot(T,EB,T,QQ1,'--',T,QQ2,'--');

%%
% EKF solution to the same problem
%
    me = m0;
    Pe = P0;
    MMe = zeros(1,length(Y));
    PPe = zeros(1,length(Y));
    ekf_steps = 5;

    for k=1:length(Y)
        dt2 = dt/ekf_steps;
        for i=1:ekf_steps
            Pe = Pe + dt2 * (2 * (1 - tanh(me)^2) * Pe + 1);
            me = me + dt2 * tanh(me);
        end
        if ~isnan(Y(k))
            me = me + Pe / (R + Pe) * (Y(k) - me);
            Pe = Pe - Pe^2/(R + Pe);
        end
        MME(:,k) = me;
        PPE(:,k) = Pe;
    end

    rmse_e = sqrt(mean((MME - X).^2))

    figure(1); clf;
    subplot(2,2,1);
    plot(T,EB,T,QQ1,'--',T,QQ2,'--');
    subplot(2,2,2);
    plot(T,MME,T,MME-1.96*sqrt(PPE),'--',T,MME+1.96*sqrt(PPE),'--');

    subplot(2,2,3);
    plot(T,EB,T,MME)

    subplot(2,2,4);
    plot(T,VB,T,PPE)

%%
% ERTS Smoother
%
    MMS = MME;
    PPS = PPE;
    ms = MME(end);
    Ps = PPE(end);
    for k=length(Y)-1:-1:1
        m = MME(k);
        P = PPE(k);
        dms = dt * tanh(m) + dt * ((1 - tanh(m)^2) * P + 1)/P * (ms - m);
        dPs = dt * 2 * ((1 - tanh(m)^2) * P + 1)/P * Ps - dt;
        ms = ms - dms;
        Ps = Ps - dPs;
        MMS(k) = ms;
        PPS(k) = Ps;
    end

    rmse_s = sqrt(mean((MMS - X).^2))

    figure(1); clf;
    plot(T,MMS,T,MMS-1.96*sqrt(PPS),'--',T,MMS+1.96*sqrt(PPS),'--');

%%
% Plot the final figures
%

    %
    % Benes-Daum
    %
    figure(1); clf; hold on

    % Plot mean
    h0 = plot(T,EB,'-k');
    set(h0,'LineWidth',1);
    set(h0,'Color',[0.5 0.5 0.5]);

    % Plot uncetainty
    h1 = fill([T'; flipud(T')], [QQ1'; flipud(QQ2')],1);
    set(h1,'EdgeColor',[.7 .7 .7],'FaceColor',[.7 .7 .7])

    % Plot signal
    h2 = plot(T,X,'-k');
    set(h2,'LineWidth',1);

    % Plot mean
    h3 = plot(T,EB,'-k');
    set(h3,'LineWidth',1);
    set(h3,'Color',[0.5 0.5 0.5]);

    % Plot observations
    h4 = plot(T,Y,'+k');

    box on
    xlabel('Time, $t$'), ylabel('Signal, $x(t)$')

    legend([h0, h1, h2, h4],'BD mean','BD 95\% quantiles','Signal','Observations')

    %
    % Benes-Daum vs EKF
    %
    figure(2); clf; hold on

    % Plot BD mean
    h1 = plot(T,EB,'-k');
    set(h1,'LineWidth',1);
    set(h1,'Color',[0.5 0.5 0.5]);

    % Plot EKF mean
    h2 = plot(T,MME,'--k');
    set(h2,'LineWidth',1);
    %set(h2,'Color',[0.0 0.0 0.0]);

    % Plot BD uncertainty
    h3 = fill([T'; flipud(T')], [QQ1'; flipud(QQ2')],1);
    set(h3,'EdgeColor',[.7 .7 .7],'FaceColor',[.7 .7 .7])

    % Plot BD mean
    h4 = plot(T,EB,'-k');
    set(h4,'LineWidth',1);
    set(h4,'Color',[0.5 0.5 0.5]);

    % Plot EKF mean
    h5 = plot(T,MME,'--k');
    set(h5,'LineWidth',1);
    set(h5,'Color',[0.0 0.0 0.0]);

    % Plot EKF uncertainty
    h6 = plot(T,MME-1.96*sqrt(PPE),'-.k',T,MME+1.96*sqrt(PPE),'-.k');
    set(h6,'LineWidth',.5);

    box on
    xlabel('Time, $t$'), ylabel('Signal, $x(t)$')
    legend([h1, h2, h3, h6(1)],'BD mean','EKF mean','BD 95\% quantiles','EKF 95\% quantiles')

    %
    % ERTS
    %
    figure(3); clf; hold on

    % Plot mean
    h0 = plot(T,MMS,'-k');
    set(h0,'LineWidth',1);
    set(h0,'Color',[0.5 0.5 0.5]);

    QQ1S = MMS-1.96*sqrt(PPS);
    QQ2S = MMS+1.96*sqrt(PPS);

    % Plot uncetainty
    h1 = fill([T'; flipud(T')], [QQ1S'; flipud(QQ2S')],1);
    set(h1,'EdgeColor',[.7 .7 .7],'FaceColor',[.7 .7 .7])

    % Plot signal
    h2 = plot(T,X,'-k');
    set(h2,'LineWidth',1);

    % Plot mean
    h3 = plot(T,MMS,'-k');
    set(h3,'LineWidth',1);
    set(h3,'Color',[0.5 0.5 0.5]);

    % Plot observations
    h4 = plot(T,Y,'+k');

    box on
    xlabel('Time, $t$'), ylabel('Signal, $x(t)$')

    legend([h0, h1, h2, h4],'ERTS mean','ERTS 95\% quantiles','Signal','Observations')


function [P2,D2]=sortem(P,D)
% this function takes in two matrices P and D, presumably the output
% from Matlab's eig function, and then sorts the columns of P to
% match the sorted columns of D (going from largest to smallest)
%
% EXAMPLE:
%
% D =
%    -90     0     0
%      0   -30     0
%      0     0   -60
% P =
%      1     2     3
%      1     2     3
%      1     2     3
%
% [P,D]=sortem(P,D)
% P =
%      2     3     1
%      2     3     1
%      2     3     1
% D =
%    -30     0     0
%      0   -60     0
%      0     0   -90

D2=diag(sort(diag(D),'descend')); % make diagonal matrix out of sorted diagonal values of input D
[c, ind]=sort(diag(D),'descend'); % store the indices of which columns the sorted eigenvalues come from
P2=P(:,ind); % arrange the columns in this order


function f = calccod(x,y,dim,wantgain,wantmeansub,wantsafe)
%
% USAGE::
%
%   f = calccod(x,y,dim,wantgain,wantmeansub,wantsafe)
%
% <x>,<y> are matrices with the same dimensions
%
% <dim> (optional) is the dimension of interest.
% default to 2 if <x> is a (horizontal) vector and to 1 if not.
% special case is 0 which means to calculate globally.
%
% <wantgain> (optional) is
%
%   - 0 means normal
%   - 1 means allow a gain to be applied to each case of <x>
%     to minimize the squared error with respect to <y>.
%     in this case, there cannot be any NaNs in <x> or <y>.
%   - 2 is like 1 except that gains are restricted to be non-negative.
%     so, if the gain that minimizes the squared error is negative,
%     we simply set the gain to be applied to be 0.
%     default: 0.
%
% <wantmeansub> (optional) is
%
%   - 0 means do not subtract any mean.  this makes it such that
%     the variance quantification is relative to 0.
%   - 1 means subtract the mean of each case of <y> from both
%     <x> and <y> before performing the calculation.  this makes
%     it such that the variance quantification
%     is relative to the mean of each case of <y>.
%     note that <wantgain> occurs before <wantmeansub>.
%     default: 1.
%
% <wantsafe> (optional) is whether to protect against NaNs. Default: 1.
%
% calculate the coefficient of determination (R^2) indicating
% the percent variance in <y> that is explained by <x>.  this is achieved
% by calculating 100*(1 - sum((y-x).^2) / sum(y.^2)).  note that
% by default, we subtract the mean of each case of <y> from both <x>
% and <y> before proceeding with the calculation.
%
% the quantity is at most 100 but can be 0 or negative (unbounded).
% note that this metric is sensitive to DC and scale and is not symmetric
% (i.e. if you swap <x> and <y>, you may obtain different results).
% it is therefore fundamentally different than Pearson's correlation
% coefficient (see calccorrelation.m).
%
% if <wantsafe> is true, NaNs are handled gracefully (a NaN causes
% that data point to be ignored). if you are sure there are no NaNs,
% you can turn off <wantsafe> to increase execution speed.
%
% if there are no valid data points (i.e. all data points are
% ignored because of NaNs), we return NaN for that case.
%
% Note some weird cases:
%
% calccod([],[]) is []
%
% Example::
%
%   x = randn(1,100);
%   calccod(x,x+0.1*randn(1,100))
%

% history:
% 2013/08/18 - fix pernicious case where <x> is all zeros and <wantgain> is 1 or 2.
% 2010/11/28 - add <wantgain>==2 case
% 2010/11/23 - changed the output range to percentages.  thus, the range is (-Inf,100].
%              also, we removed the <wantr> input since it was dumb.

% input
if ~exist('dim','var') || isempty(dim)
  if isvector(x) && size(x,1)==1
    dim = 2;
  else
    dim = 1;
  end
end
if ~exist('wantgain','var') || isempty(wantgain)
  wantgain = 0;
end
if ~exist('wantmeansub','var') || isempty(wantmeansub)
  wantmeansub = 1;
end
if ~exist('wantsafe','var') || isempty(wantsafe)
  wantsafe = 1;
end

% handle weird case up front
if isempty(x)
  f = [];
  return;
end

% handle 0 case
if dim==0
  x = x(:);
  y = y(:);
  dim = 1;
end

% handle gain
if wantgain
  % to get the residuals, we want to do something like y-x*inv(x'*x)*x'*y
  temp = 1./dot(x,x,dim) .* dot(x,y,dim);
  if wantgain==2
    temp(temp < 0) = 0;  % if the gain was going to be negative, rectify it to 0.
  end
  x = bsxfun(@times,x,temp);
end

% propagate NaNs (i.e. ignore invalid data points)
if wantsafe
  x(isnan(y)) = NaN;
  y(isnan(x)) = NaN;
end

% handle mean subtraction
if wantmeansub
  mn = nanmean(y,dim);
  y = bsxfun(@minus,y,mn);
  x = bsxfun(@minus,x,mn);
end

% finally, compute it
f = 100*(1 - zerodiv(nansum((y-x).^2,dim),nansum(y.^2,dim),NaN,0));

% JUNK:
%
% % <wantr> (optional) is whether to apply signedarraypower(f,0.5)
% %   at the very end, giving something like a correlation coefficient (r).
% %   default: 1.
%
% if ~exist('wantr','var') || isempty(wantr)
%   wantr = 1;
% end
%
% if wantr
%   f = signedarraypower(f,0.5);
% end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% calculation of the 'epsgg' matrix for circular holes using
%%% analytical expression; the matrix is symmetric, i.e E'=E
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function epsi=epsgg(r, na, nb, b1, b2, N1, N2)
N = N1*N2;
epsilon=zeros(N1,N2);
epsi=zeros(N,N);
%%% filling factor, ratio between the area occupied by the cylinders and area of the unit cell
f=(2*pi/sqrt(3))*r^2;

for  l=1:N1
      for m=1:N2
          for  n=1:N1
              for p=1:N2
                  GGx=(l-n)*b1(1)+(m-p)*b2(1);
                  GGy=(l-n)*b1(2)+(m-p)*b2(2);
                  GG=sqrt(GGx^2+GGy^2); x=2*pi*GG;
                  %%% GG is a scalar which changes with iteration and goes into Bessel function argument
                  if (GG~=0)
                      epsilon(p,n)=2*f*(na^2-nb^2)*besselj(1,x*r)/(x*r);
                  else
                      epsilon(p,n)=f*na^2+(1-f)*nb^2;
                  end ;
              end;
           end;
           u=(l-1)*N2+m; %%% this is the line index
           epsi(u,:)=reshape(epsilon,1,N);
        end;
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%  LU 分解的紧凑方式
function [L,U,x] = my_lu(A,b)

[m,n]=size(A);
if (m~=n)
    fprintf('\n Error: A 不是方阵!\n'); return;
end

L = eye(n);
U = zeros(n);

for k = 1:n
    U(k,k) = A(k,k)-sum(L(k,1:k-1)'.*U(1:k-1,k));
    if (U(k,k) == 0)
        fprintf('\n Error: 第 %d 个主元＝0 !\n',k); return;
    end
    for j = k+1:n
        U(k,j) = A(k,j) - sum(L(k,1:k-1)'.*U(1:k-1,j));
        L(j,k) = (A(j,k) - sum( L(j,1:k-1)'.*U(1:k-1,k) ))/U(k,k);
    end
end

% 求解Ly=b 和 Ux=y
y(1) = b(1);
for i=2:n
    summary = 0;
    for t=1:i-1
        summary = summary + L(i,t) * y(t);
    end
    y(i)=b(i) - summary;
end
x(n) = y(n)/U(n,n);
for i = n-1 : -1 : 1
    summary = 0;
    for t=i+1:n
        summary = summary + U(i,t) * x(t);
    end
    x(i)=(y(i) - summary)/U(i,i);
end

% END


% Program to form Admittance And Impedance Bus Formation....

function ybus = ybus();  % Returns ybus

linedata = linedata6(); % Calling "linedata6.m" for Line Data...
fb = linedata(:,1);     % From bus number...
tb = linedata(:,2);     % To bus number...
r = linedata(:,3);      % Resistance, R...
x = linedata(:,4);      % Reactance, X...
b = linedata(:,5);      % Ground Admittance, B/2...
z = r + i*x;            % Z matrix...
y = 1./z;               % To get inverse of each element...
b = i*b;                % Make B imaginary...

nbus = max(max(fb),max(tb));    % no. of buses...
nbranch = length(fb);           % no. of branches...
ybus = zeros(nbus,nbus);        % Initialise YBus...

 % Formation of the Off Diagonal Elements...
 for k=1:nbranch
     ybus(fb(k),tb(k)) = -y(k);
     ybus(tb(k),fb(k)) = ybus(fb(k),tb(k));
 end

 % Formation of Diagonal Elements....
 for m=1:nbus
     for n=1:nbranch
         if fb(n) == m | tb(n) == m
             ybus(m,m) = ybus(m,m) + y(n) + b(n);
         end
     end
 end
 ybus;                  % Bus Admittance Matrix
 zbus = inv(ybus);      % Bus Impedance Matrix