function [ n_data, n, x, fx ] = legendre_poly_values ( n_data )

%*****************************************************************************80
%
%% LEGENDRE_POLY_VALUES returns values of the Legendre polynomials.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      LegendreP [ n, x ]
%
%  Differential equation:
%
%    (1-X*X) * P(N,X)'' - 2 * X * P(N,X)' + N * (N+1) = 0
%
%  First terms:
%
%    P( 0,X) =       1
%    P( 1,X) =       1 X
%    P( 2,X) =  (    3 X^2 -       1)/2
%    P( 3,X) =  (    5 X^3 -     3 X)/2
%    P( 4,X) =  (   35 X^4 -    30 X^2 +     3)/8
%    P( 5,X) =  (   63 X^5 -    70 X^3 +    15 X)/8
%    P( 6,X) =  (  231 X^6 -   315 X^4 +   105 X^2 -     5)/16
%    P( 7,X) =  (  429 X^7 -   693 X^5 +   315 X^3 -    35 X)/16
%    P( 8,X) =  ( 6435 X^8 - 12012 X^6 +  6930 X^4 -  1260 X^2 +   35)/128
%    P( 9,X) =  (12155 X^9 - 25740 X^7 + 18018 X^5 -  4620 X^3 +  315 X)/128
%    P(10,X) =  (46189 X^10-109395 X^8 + 90090 X^6 - 30030 X^4 + 3465 X^2-63 ) /256
%
%  Recursion:
%
%    P(0,X) = 1
%    P(1,X) = X
%    P(N,X) = ( (2*N-1)*X*P(N-1,X)-(N-1)*P(N-2,X) ) / N
%
%    P'(0,X) = 0
%    P'(1,X) = 1
%    P'(N,X) = ( (2*N-1)*(P(N-1,X)+X*P'(N-1,X)-(N-1)*P'(N-2,X) ) / N
%
%  Formula:
%
%    P(N,X) = (1/2**N) * sum ( 0 <= M <= N/2 ) C(N,M) C(2N-2M,N) X^(N-2*M)
%
%  Orthogonality:
%
%    Integral ( -1 <= X <= 1 ) P(I,X) * P(J,X) dX
%      = 0 if I =/= J
%      = 2 / ( 2*I+1 ) if I = J.
%
%  Approximation:
%
%    A function F(X) defined on [-1,1] may be approximated by the series
%
%      C0*P(0,X) + C1*P(1,X) + ... + CN*P(N,X)
%
%    where
%
%      C(I) = (2*I+1)/(2) * Integral ( -1 <= X <= 1 ) F(X) P(I,X) dx.
%
%  Special values:
%
%    P(N,1) = 1.
%    P(N,-1) = (-1)**N.
%    | P(N,X) | <= 1 in [-1,1].
%
%    P(N,0,X) = P(N,X), that is, for M=0, the associated Legendre
%    function of the first kind and order N equals the Legendre polynomial
%    of the first kind and order N.
%
%    The N zeroes of P(N,X) are the abscissas used for Gauss-Legendre
%    quadrature of the integral of a function F(X) with weight function 1
%    over the interval [-1,1].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the order of the function.
%
%    Output, real X, the point where the function is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 22;

  fx_vec = [ ...
      0.1000000000000000E+01, ...
      0.2500000000000000E+00, ...
     -0.4062500000000000E+00, ...
     -0.3359375000000000E+00, ...
      0.1577148437500000E+00, ...
      0.3397216796875000E+00, ...
      0.2427673339843750E-01, ...
     -0.2799186706542969E+00, ...
     -0.1524540185928345E+00, ...
      0.1768244206905365E+00, ...
      0.2212002165615559E+00, ...
      0.0000000000000000E+00, ...
     -0.1475000000000000E+00, ...
     -0.2800000000000000E+00, ...
     -0.3825000000000000E+00, ...
     -0.4400000000000000E+00, ...
     -0.4375000000000000E+00, ...
     -0.3600000000000000E+00, ...
     -0.1925000000000000E+00, ...
      0.8000000000000000E-01, ...
      0.4725000000000000E+00, ...
      0.1000000000000000E+01 ];

  n_vec = [ ...
     0,  1,  2, ...
     3,  4,  5, ...
     6,  7,  8, ...
     9, 10,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3 ];

  x_vec = [ ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.00E+00, ...
     0.10E+00, ...
     0.20E+00, ...
     0.30E+00, ...
     0.40E+00, ...
     0.50E+00, ...
     0.60E+00, ...
     0.70E+00, ...
     0.80E+00, ...
     0.90E+00, ...
     1.00E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function [ value, seed ] = uniform_01_sample ( seed )

%*****************************************************************************80
%
%% UNIFORM_01_SAMPLE is a portable random number generator.
%
%  Formula:
%
%    SEED = SEED * (7**5) mod ( 2**31 - 1 )
%    UNIFORM_01_SAMPLE = SEED * / ( 2**31 - 1 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 August 2007
%
%  Parameters:
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real VALUE, a random value between 0 and 1.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
%  Local parameters:
%
%    IA = 7**5
%    IB = 2**15
%    IB16 = 2**16
%    IP = 2**31-1
%
  ia = 16807;
  ib15 = 32768;
  ib16 = 65536;
  ip = 2147483647;
%
%  Don't let SEED be 0 or IP
%
  if ( seed == 0 | seed == ip )
    seed = floor ( ip / 2 );
  end
%
%  Get the 15 high order bits of SEED.
%
  ixhi = floor ( seed / ib16 );
%
%  Get the 16 low bits of SEED and form the low product.
%
  loxa = ( seed - ixhi * ib16 ) * ia;
%
%  Get the 15 high order bits of the low product.
%
  leftlo = floor ( loxa / ib16 );
%
%  Form the 31 highest bits of the full product.
%
  iprhi = ixhi * ia + leftlo;
%
%  Get overflow past the 31st bit of full product.
%
  k = floor ( iprhi / ib15 );
%
%  Assemble all the parts and presubtract IP.  The parentheses are essential.
%
  seed = ( ( ( loxa - leftlo * ib16 ) - ip ) + ( iprhi - k * ib15 ) * ib16 ) + k;
%
%  Add IP back in if necessary.
%
  if ( seed < 0 )
    seed = seed + ip;
  end
%
%  Multiply by 1 / (2**31-1).
%
  value = seed * 4.656612875E-10;

  return
end


% This subroutine assigns creates a 3D grid with
% spacing dx,dy, dz (in degreees). The size will
% be selected interactiVELY. The pvalue in each
% volume around a grid point containing ni earthquakes
% will be calculated as well as the magnitude
% of completness
%   Stefan Wiemer 1/98

report_this_filefun(mfilename('fullpath'));
global no1 bo1 inb1 inb2

if sel == 'in'
    % get the grid parameter
    % initial values
    %
    dx = 0.1;
    dy = 0.1 ;
    dz = 5.00 ;
    ni = 300;
    R = 10000;

    def = {'1982.6','3','200','0.1','0.1',num2str(dz),num2str(max(a.Depth)), num2str(min(a.Depth))};

    tit ='Three dimesional z-value analysis';
    prompt={'Time of analysis?',...
        'Window length in years ?',...
        'Sample size N?',...
        'Spacing in Longitude (dx in [deg])',...
        'Sapcing in Latitude  (dy in [deg])',...
        'Spacing in Depth    (dz in [km ])',...
        'Depth Range: deep limit [km] ',...
        'Depth Range: shallow limit',...
        };

    ni2 = inputdlg(prompt,tit,1,def);

    l = ni2{1}; ti  = str2double(l);
    l = ni2{2}; ZG.compare_window_yrs= str2double(l);
    l = ni2{3}; ni= str2double(l);
    l = ni2{4}; dx= str2double(l);
    l = ni2{5}; dy= str2double(l);
    l = ni2{6}; dz= str2double(l);
    l = ni2{7}; z1= str2double(l);
    l = ni2{8}; z2= str2double(l);

    sel = 'ca'; zgrid3db

end   % if sel == 'in'

% get the grid-size interactively and
% calculate the b-value in the grid by sorting
% thge seimicity and selectiong the ni neighbors
% to each grid point

if sel == 'ca'
    selgp3dB
    zvect=[z2:dz:z1];
    gz = zvect;
    itotal = length(gx)*length(gz)*length(gy);
    zmap_message_center.set_info(' ','Running... ');think
    %  make grid, calculate start- endtime etc.  ...
    %
    zvg = ones(length(gx),length(gy),length(gz))*nan;
    ra  = ones(length(gx),length(gy),length(gz));

    t0b = min(a.Date)  ;
    n = a.Count;
    teb = a(n,3) ;
    tdiff = round((teb - t0b)*365/par1);
    loc = zeros(3, length(gx)*length(gy));

    % loop over  all points
    %
    i2 = 0.;
    i1 = 0.;
    allcount = 0.;
    wai = waitbar(0,' Please Wait ...  ');
    set(wai,'NumberTitle','off','Name',' 3D gridding - percent done');;
    drawnow
    %
    %
    tl = teb - t0b;
    iwl = floor(ZG.compare_window_yrs*365/par1);
    t = floor((ti-t0b)*365/par1);

    z0 = 0; x0 = 0; y0 = 0; dt = 1;
    % loop over all points
    for x = min(gx):dx:max(gx)
        x0 = x0+1;
        for y = min(gy):dy:max(gy)
            y0 = y0+1;
            for z = min(gz):dz:max(gz)
                z0 = z0+1;
                allcount = allcount + 1.;
                i2 = i2+1;

                % calculate distance from center point and sort wrt distance
                l = sqrt(((a.Longitude-x)*cos(pi/180*y)*111).^2 + ((a.Latitude-y)*111).^2 + ((a.Depth - z)).^2 ) ;
                [s,is] = sort(l);
                b = a(is(:,1),:) ;       % re-orders matrix to agree row-wise

                % take first ni points
                b = b(1:ni,:);      % new data per grid point (b) is sorted in distance

                [bv magco stan av me mer me2,  pr] =  bvalca3(b,inb1,inb2);
                l2 = sort(l);
                zvg(x0,y0,z0) = bv;
                ra(x0,y0,z0) = l2(ni);

                zvg(x0,y0,z0) = (mean1 - mean2)/(sqrt(var1/(ncu-iwl)+var2/iwl));
                ra(x0,y0,z0) = l(ni);
                waitbar(allcount/itotal)
            end  % for z
            z0 = 0;
        end  % for y
        y0 = 0;
    end  % for x
    x0 = 0;

    % save data
    %
    catSave3 =...
        [ 'zmap_message_center.set_info(''Save Grid'',''  '');think;',...
        '[file1,path1] = uiputfile(fullfile(hodi, ''eq_data'', ''*.mat''), ''Grid Datafile Name?'') ;',...
        ' sapa2 = [''save '' path1 file1 '' zvg ra gx gy gz dx dy dz d par1 tdiff t0b teb a main faults mainfault coastline yvect xvect tmpgri ll''];',...
        ' if length(file1) > 1, eval(sapa2),end , done']; eval(catSave3)

    close(wai)
    watchoff

    gz = -gz;
    zv2 = zvg;
    ac2 = 'new'; myslicer;

end  % if cal


% EX_STOKES_DRIVEN_CAVITY_3D_TH: solve the Stokes problem in the driven cavity with generalized Taylor-Hood elements.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined from the aspect ratio using the NURBS toolbox
aspect_ratio=[1.5 0.3];
nrb_section = nrb4surf([0 0], [1 0], [0 aspect_ratio(1)], [1 aspect_ratio(1)]);
problem_data.geo_name = nrbextrude (nrb_section, [0 0 aspect_ratio(2)]);

% Type of boundary conditions for each side of the domain
problem_data.drchlt_sides = 1:6;
problem_data.nmnn_sides = [];

% Physical parameters
problem_data.viscosity = @(x, y, z) ones (size (x));

% Force and boundary terms
problem_data.f  = @(x, y, z) zeros ([3, size(x)]);
problem_data.h  = @test_stokes_3d_symdrivcav_h_drchlt;

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.element_name = 'th';       % Element type for discretization
method_data.degree       = [2  2  2];  % Degree of the splines (pressure space)
method_data.regularity   = [1  1  1];  % Regularity of the splines (pressure space)
method_data.nsub         = [5  5  5];  % Number of subdivisions
method_data.nquad        = [4  4  4];  % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER
[geometry, msh, space_v, vel, space_p, press] = ...
                       solve_stokes (problem_data, method_data);

% 4) POST-PROCESSING
% 4.1) EXPORT TO PARAVIEW
output_file = 'Driven_cavity_3d_TH_Deg2_Reg1_Sub5';

fprintf ('The result is saved in the files %s \n and %s \n \n', ...
           [output_file '_vel'], [output_file '_press']);
vtk_pts = {linspace(0, 1, 15), linspace(0, 1, 15), linspace(0, 1, 15)};
sp_to_vtk (press, space_p, geometry, vtk_pts, [output_file '_press'], 'press')
sp_to_vtk (vel,   space_v, geometry, vtk_pts, [output_file '_vel'  ], {'velocity', 'divergence'}, {'value', 'divergence'})

%!test
%! aspect_ratio=[1.5 0.3];
%! nrb_section = nrb4surf([0 0], [1 0], [0 aspect_ratio(1)], [1 aspect_ratio(1)]);
%! problem_data.geo_name = nrbextrude (nrb_section, [0 0 aspect_ratio(2)]);
%! problem_data.drchlt_sides = 1:6;
%! problem_data.nmnn_sides = [];
%! problem_data.viscosity = @(x, y, z) ones (size (x));
%! problem_data.f  = @(x, y, z) zeros ([3, size(x)]);
%! problem_data.h  = @test_stokes_3d_symdrivcav_h_drchlt;
%! method_data.element_name = 'th';       % Element type for discretization
%! method_data.degree       = [2  2  2];  % Degree of the splines (pressure space)
%! method_data.regularity   = [1  1  1];  % Regularity of the splines (pressure space)
%! method_data.nsub         = [5  5  5];  % Number of subdivisions
%! method_data.nquad        = [4  4  4];  % Points for the Gaussian quadrature rule
%! [geometry, msh, space_v, vel, space_p, press] = ...
%!                        solve_stokes (problem_data, method_data);
%! assert (msh.nel, 125)
%! assert (space_p.ndof, 343)
%! assert (space_v.ndof, 5184)


%This Matlab script can be used to reproduce Figure 2.6 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Number of BS antennas
M = 100;

%Set the angle of the UE
theta = pi/6;

%Set the ASD
ASD = 10;

%Define the antenna spacing (in number of wavelengths)
antennaSpacing = 1/2; %Half wavelength distance

%Compute spatial correlation matrix with local scattering model and
%different angular distributions
R_Gaussian = functionRlocalscattering(M,theta,ASD,antennaSpacing,'Gaussian');
R_Uniform = functionRlocalscattering(M,theta,ASD,antennaSpacing,'Uniform');
R_Laplace = functionRlocalscattering(M,theta,ASD,antennaSpacing,'Laplace');

%Channel correlation matrix with uncorrelated fading
R_uncorrelated = eye(M);

%Extract the eigenvalues and place them in decreasing order
eigenvalues_Gaussian = flipud(eig(R_Gaussian));
eigenvalues_Uniform = flipud(eig(R_Uniform));
eigenvalues_Laplace = flipud(eig(R_Laplace));
eigenvalues_uncorr = flipud(eig(R_uncorrelated));

%Replace negative eigenvalues with a small positive number
%(since the correlation matrices should be Hermitian)
eigenvalues_Gaussian(eigenvalues_Gaussian<0) = 1e-16;
eigenvalues_Uniform(eigenvalues_Uniform<0) = 1e-16;
eigenvalues_Laplace(eigenvalues_Laplace<0) = 1e-16;

%% Plot the simulation results
figure;
hold on; box on;

plot(1:M,10*log10(eigenvalues_Laplace),'r--','LineWidth',1);
plot(1:M,10*log10(eigenvalues_Uniform),'b-.','LineWidth',1);
plot(1:M,10*log10(eigenvalues_Gaussian),'k','LineWidth',1);
plot(1:M,10*log10(eigenvalues_uncorr),'k:','LineWidth',1);

xlabel('Eigenvalue number in decreasing order');
ylabel('Normalized eigenvalue [dB]');
legend('Laplace','Uniform','Gaussian','Location','SouthEast');
ylim([-50 10]);


%STEREORECTIFYUNCALIBRATED  Computes a rectification transform for an uncalibrated stereo camera
%
%     [H1,H2,success] = cv.stereoRectifyUncalibrated(points1, points2, F, imageSize)
%     [...] = cv.stereoRectifyUncalibrated(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __points1__ Array of feature points in the first image as a cell array of
%   2-element vectors: `{[x1, y1], [x2, y2], ...}` or an Nx2/Nx1x2/1xNx2
%   numeric array. The same formats as in cv.findFundamentalMat are supported.
% * __points2__ The corresponding points in the second image, same size and
%   type as `points1`.
% * __F__ Input 3x3 fundamental matrix. It can be computed from the same set
%   of point pairs using cv.findFundamentalMat.
% * __imageSize__ Size of the image `[w,h]`.
%
% ## Output
% * __H1__ 3x3 rectification homography matrix for the first image.
% * __H2__ 3x3 rectification homography matrix for the second image.
% * __success__ success flag. Returns true if successfull, false otherwise.
%
% ## Options
% * __Threshold__ Optional threshold used to filter out the outliers. If the
%   parameter is greater than zero, all the point pairs that do not comply
%   with the epipolar geometry (that is, the points for which
%   `|points2{i}' * F * points1{i}| > Threshold`) are rejected prior to
%   computing the homographies. Otherwise, all the points are considered
%   inliers. default 5
%
% The function computes the rectification transformations without knowing
% intrinsic parameters of the cameras and their relative position in the
% space, which explains the suffix "uncalibrated". Another related difference
% from cv.stereoRectify is that the function outputs not the rectification
% transformations in the object (3D) space, but the planar perspective
% transformations encoded by the homography matrices `H1` and `H2`. The
% function implements the algorithm [Hartley99].
%
% ### Note
% While the algorithm does not need to know the intrinsic parameters of the
% cameras, it heavily depends on the epipolar geometry. Therefore, if the
% camera lenses have a significant distortion, it would be better to correct
% it before computing the fundamental matrix and calling this function. For
% example, distortion coefficients can be estimated for each head of stereo
% camera separately by using cv.calibrateCamera. Then, the images can be
% corrected using cv.undistort, or just the point coordinates can be corrected
% with cv.undistortPoints.
%
% ## References
% [Hartley99]:
% > Richard I Hartley. "Theory and practice of projective rectification".
% > International Journal of Computer Vision, 35(2):115-127, 1999.
%
% See also: cv.stereoCalibrate, cv.stereoRectify, cv.calibrateCamera,
%  cv.undistort, cv.undistortPoints, estimateUncalibratedRectification,
%  rectifyStereoImages
%


function pass = test_transpose( )
% Test transpose and ctranspose

tol = 10*chebfunpref().cheb2Prefs.chebfun2eps;

% Test empty diskfunv.
f = diskfunv;
pass(1) = isempty(f');
pass(2) = isempty(f.');

% Test function
f = diskfun(@(x,y) cos((x+.1).*y));
% diskfunv
u = grad(f);
[m,n,p] = size(u');
pass(3) = all( ([isinf(m) isinf(n) p==2]) );
[m,n,p] = size(u.');
pass(4) = all( ([isinf(m) isinf(n) p==2]) );

% Check transpose and ctranspose give the same results for real-valued
% diskfunv objects.
w = u'-u.';
rng(10); th0 = rand; r0 = rand;
pass(5) = ( norm(w(th0,r0, 'polar')) < tol );

% Check transpose of transpose is the original u.
v = u.';
pass(6) = ( norm(u-v.') < tol );

% Check u'*u is a diskfun.
pass(7) = isa(u'*u,'diskfun');

end


%% Implementation of Progressive Switching Median Filter
%% Base Paper : Zhou Wang and David Zhang, "Progressive Switching Median
%% Filter for the Removal of Impulse Noise from Highly Corrupted Images",
%% IEEE Trans. on Cir. and Sys., vol. 46, no. 1, Jan. 1999.
%% Function Y = PSMF(x)
%% input    x = Image is corrupted by Salt & Pepper Noise
%%
%%  Example: Y = PSMF(x);
%%      Posted date   : 16 - 10 - 2008
%%      Modified date :
%%
%% Developed By : K.Kannan (kannan.keizer@gmail.com)
%%                & Jeny Rajan (jenyrajan@gmail.com)
%%                  Medical Imaging Research Group (MIRG), NeST,
%%                  Trivandrum.
%% Progressive Switching Median Filter
function Y = PSMF(x)
x = double(x);
WF = 3; ND = 3;T = 40;a = 65;b = -50;
M = medfilt2(x,[3 3]);
N = abs(x - M);
N(N>T)=0;
N = N ~= 0;
N = double(N);
R = sum(N(:))/(size(x,1) * size(x,2));
if R <= 0.25
    WD = 3;
else
    WD = 5;
end
TD = a + (b * R);
z = IMPDET(x,ND,WD,TD);
Y = NF(x,z,WF);

%% Impulse Detection
function F1 = IMPDET(x,ND,WD,TD)
X = x;
M = medfilt2(X,[WD WD]);
D = abs(X - M);
F = zeros(size(x));
F(D>=TD)=1;
F1 = F;
X(F1==F)=X(F1==F);
X(F1~=F)=M(F1~=F);
for i = 1:ND-1
    M = medfilt2(X,[WD WD]);
    F1(abs(X - M)<TD)=F(abs(X - M)<TD);
    F1((X - M)>=TD)=1;
    X(F1==F)=X(F1==F);
    X(F1~=F)=M(F1~=F);
    F = F1;
end
return;

%% Noise Filtering
function Y = NF(x,f,WF)
g = f;
Y = x;
Y1 = Y;
g1 = g;
s = sum(g(:));
while s ~= 0
    M = medfilt2(Y,[WF WF]);
    Y1(g==1)=M(g==1);
    g1(Y~=Y1)=0;
    Y = Y1;
    g = g1;
    s1 = sum(g(:));
    if s1 ~= s
        s = s1;
    else
        s = 0;
    end
end
return;


function [Vertices_sm, A] = tess_smooth(Vertices, a, nIterations, VertConn, isKeepSize, Faces)
% TESS_SMOOTH: Smooths a surface.
%
% USAGE:  [Vertices_sm, A] = tess_smooth(Vertices, a, nIterations, VertConn, isKeepSize)
%         [Vertices_sm, A] = tess_smooth(Vertices, a, nIterations, VertConn)
%
% INPUT:
%    - Vertices    : [N,3] vertices of matrix to smooth
%    - a           : scalar smooth weighting parameter (0-1 less-more smoothing)
%    - nIterations : number of times to apply the smoothing
%    - VertConn    : Vertex connectivity sparse matrix
%    - isKeepSize  : If 1, the final surface is scaled so that the convex envelope
%                    has the same bounding box as the initial surface
% OUTPUT:
%    - Vertices_sm : vertices list of smoothed surface
%    - A           : smoothing matrix

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2012-2013

% Parse inputs
if (nargin < 5) || isempty(isKeepSize)
    isKeepSize = 1;
end
% Check matrices orientation
if (size(Vertices, 2) ~= 3)
    error('Vertices must have 3 columns (X,Y,Z).');
end

% Get initial bounding box
if isKeepSize
    initBounds = [min(Vertices); max(Vertices)];
end
% Calculate smoothing matrix
A = spones(VertConn);
sumA = sum(A);
sumA(sumA==0) = eps;
A = spdiags((a./sumA)', 0, size(A,1), size(A,2)) * A;
A = A + spdiags((1-a) * ones(size(A,1), 1), 0, size(A,1), size(A,2));

% Smooth vertices matrix
Vertices_sm = double(Vertices);
for i = 1:nIterations
    Vertices_sm = A * Vertices_sm;
end

% Scale final surface
if isKeepSize
    % Compute scale
    finalBounds = [min(Vertices_sm); max(Vertices_sm)];
    initBounds = initBounds - repmat(mean(Vertices),2,1);
    scale = diff(initBounds,[],1) ./ diff(finalBounds,[],1);
    % Center and apply scale
    center = mean(Vertices_sm);
    Vertices_sm = bst_bsxfun(@minus, Vertices_sm, center);
    Vertices_sm = bst_bsxfun(@times, Vertices_sm, scale);
    Vertices_sm = bst_bsxfun(@plus,  Vertices_sm, center);

%     % Compute normals
%     VertNormals = tess_normals(Vertices_sm, Faces, VertConn);
%     Vertices_sm = Vertices_sm + 0.00001 * nIterations * VertNormals;
end


% GRIFFIN_LIM The Griffin & Lim phase retrieval algorithm
%
% Usage
%    x = griffin_lim(x_init, x_phi, x_psi_mod, filters, options)
%
% Input
%    x_init (numeric): The inital guess for x.
%    x_phi (numeric): The lowpass coefficients
%    x_psi_mod (cell): The wavelet modulus coefficients.
%    filters (struct): The filter bank used to calculate the wavelet transform
%        in WAVELET_1D.
%    options (struct, optional): Different parameters to the algorithm and to
%        WAVELET_1D and INVERSE_WAVELET_1D that are called. Parameters for
%        GRIFFIN_LIM are:
%            gl_iter (numeric): The number of iterations (default 32).
%            verbose (boolean): If true, shows computation information (default
%               false).
%            x_phi_resolution (numeric): The resolution of the x_phi input,
%               with respect to the maximum resolution allowed by filters
%               (default 0).
%
% Output
%     x (numeric): The result of the algorithm.
%
% Description
%    Given the lowpass coefficients and the modulus of the wavelet
%    coefficients, the Griffin & Lim algorithm attempts to reconstruct the
%    original signal by recovering the phase of the wavelet coefficients. To
%    do this, it performs an alternating projection on the wavelet reproducing
%    kernel (the set of coefficients that are valid wavelet transforms) and
%    the set of coefficients of the desired modulus. Since the latter is a
%    non-convex set, the algorithm will not converge. In some applications,
%    however, the approximation that it provides is sufficient. For more
%    details, see [1].
%
% References
%    [1] D. W. Grifﬁn and J. S. Lim, “Signal estimation from modiﬁed short-
%        time fourier transform,” IEEE Trans. Acoust., Speech, Signal
%        Process., vol. 32, no. 2, pp. 236–243, 1984.
%
% See also
%    WAVELET_1D, INVERSE_WAVELET_1D

function x = griffin_lim(x_init, x_phi, x_psi_mod, filters, options)
	if nargin < 5
		options = struct();
	end

	options = fill_struct(options, 'gl_iter', 32);
	options = fill_struct(options, 'verbose', 0);
	options = fill_struct(options, 'x_phi_resolution', 0);

	dual_filters = dual_filter_bank(filters);

	x0 = x_init;
	[x0_phi, x0_psi, meta_phi, meta_psi] = wavelet_1d(x0, filters, options);

	for k = 1:length(filters.psi.filter)
		if isempty(x_psi_mod{k})
			continue;
		end
		ds = length(x_psi_mod{k})/length(x0_psi{k});
		if ds > 1
			x_psi_mod{k} = x_psi_mod{k}(1:ds:end)*sqrt(ds);
		else
			x_psi_mod{k} = upsample(x_psi_mod{k}, length(x0_psi{k}));
		end
	end

	for iter = 1:options.gl_iter
		if options.verbose, fprintf('%d...',iter); end
		for k = 1:length(filters.psi.filter)
			if isempty(x_psi_mod{k})
				x1_psi{k} = x0_psi{k};
			else
				x1_psi{k} = x0_psi{k}.*x_psi_mod{k}./abs(x0_psi{k});
			end
		end

		meta_phi.resolution = options.x_phi_resolution;

		x1 = inverse_wavelet_1d(length(x_init), x_phi, x1_psi, meta_phi, ...
			meta_psi, dual_filters, options);
		[x0_phi, x0_psi, meta_phi, meta_psi] = wavelet_1d(x1, filters, ...
			options);
	end
	if options.verbose, fprintf('\n'); end

	x = x1;
end


function traject = compute_vf_trajectory(points0, vf, options)

% compute_vf_trajectory - compute the trajectories of a vector field
%
%   traject = compute_vf_trajectory(points0, vf, options);
%
%   For each point points0(:,i) in R^2, traject(:,j,i) is the jth point
%   along the trajectory (x(t),y(t)) satisfying
%       d/dt (x,y) = vf(x,y,:)
%
%   This function is very similar to perform_vf_integration.
%
%   The number points along the trajectory is options.niter
%   The timestep is options.dt
%
%   Copyright (c) 2007 Gabriel Peyr?

options.null = 0;
if isfield(options, 'niter')
    niter = options.niter;
else
    niter = 200;
end
if isfield(options, 'dt')
    dt = options.dt;
else
    dt = 0.2;
end

npoints = size(points0,2);
n = size(vf,1);
m = size(vf,2);

traject = zeros(2,niter+1,npoints);
traject(:,1,:) = reshape(points0, [2 1 npoints]);
points = points0;
for i=1:niter
    progressbar(i,niter);
    % evaluate the gradient at the location
    gx = interp2( vf(:,:,1), points(2,:), points(1,:) );
    gy = interp2( vf(:,:,2), points(2,:), points(1,:) );
    points = points + dt * [gx;gy];
    points(1,:) = clamp(points(1,:), 1,n);
    points(2,:) = clamp(points(2,:), 1,m);
    traject(:,i+1,:) = reshape(points, [2 1 npoints]);
end


% Shark Demo
%
% Copyright (c) by Lorenzo Torresani, Stanford University
%
% A demo of Non-Rigid Structure From Motion on artificial shark sequence
%
%
% The 3D reconstruction technique is based on the following paper:
%
%  Lorenzo Torresani, Aaron Hertzmann and Christoph Bregler,
%     Learning Non-Rigid 3D Shape from 2D Motion, NIPS 16, 2003
%  http://cs.stanford.edu/~ltorresa/projects/learning-nr-shape/
%
%
% Function em_sfm implements the algorithms "EM-Gaussian" and "EM-LDS" described
% in the paper
%
% I recommend that you try to compile the CMEX code for the function computeH:
% type 'mex computeH.c' in the Matlab Command Window ('mex computeH.c -l matlb' under Unix)
%

% loads the matrix P3_gt containing the ground thruth data: P3_gt([t t+T t+2*T],:) contains the 3D coordinates of the J points at time t
% (T is the number of frames, J is the number of points)
load('jaws.mat');
[T, J] = size(P3_gt); T = T/3;

% 2D motion resulting from orthographic projection (Eq (1))
p2_obs = P3_gt(1:2*T, :);

% runs the non-rigid structure from motion algorithm
use_lds = 1;
max_em_iter = 60;
tol = 0.0001;
K = 2; % number of deformation shapes
Zcoords_gt = P3_gt(2*T+1:3*T,:) - mean(P3_gt(2*T+1:3*T,:),2)*ones(1,J);
Zdist = max(Zcoords_gt,[],2) - min(Zcoords_gt,[],2); % size of the 3D shape along the Z axis for each time frame
MD = zeros(T,J);

[P3, S_hat, V, RO, Tr, Z] = em_sfm(p2_obs, MD, K, use_lds, tol, max_em_iter);

%% Compares it with ground truth.
% Note that there are still 2 unresolvable ambiguities:
% 1. depth direction (i.e. the shape could be "flipped" along the Z axis) -> we test both possibilities
% 2. Z translation                                                        -> we subtract the mean of the Z coords to evaluate reconstruction results
Zcoords_em = P3(2*T+1:3*T,:) - mean(P3(2*T+1:3*T,:),2)*ones(1,J);

Zerror1 = mean( mean(abs(Zcoords_em - Zcoords_gt), 2)./Zdist );
Zerror2 = mean( mean(abs(-Zcoords_em - Zcoords_gt), 2)./Zdist );

if Zerror2 < Zerror1,
   avg_zerror = 100*Zerror2;
   P3(2*T+1:3*T,:) = -(P3(2*T+1:3*T,:) - mean(P3(2*T+1:3*T,:),2)*ones(1,J));
else
   avg_zerror = 100*Zerror1;
   P3(2*T+1:3*T,:) = P3(2*T+1:3*T,:) - mean(P3(2*T+1:3*T,:),2)*ones(1,J);
end
fprintf('Average reconstruction error in Z: %f%%\n', avg_zerror);

vis_reconstruction(P3_gt, P3);


function x = cs_qrsol (A,b,order)                                           %#ok
%CS_QRSOL solve a sparse least-squares problem.
%   x = cs_qrsol(A,b) solves the over-determined least squares problem to
%   find x that minimizes norm(A*x-b), where b is a full vector and
%   A is m-by-n with m >= n.  If m < n, it solves the underdetermined system
%   Ax=b.  A 3rd input argument specifies the ordering method to use
%   (0: natural, 3: amd(A'*A)).  The default ordering is 3.
%
%   Example:
%       Prob = UFget ('HB/well1033') ; A = Prob.A ; [m n] = size (A) ;
%       b = rand (m,1) ;
%       x1 = cs_qrsol (A,b) ;
%       x2 = A\b ;
%       norm (x1-x2)
%
%   For this example, cs_qrsol is about 3 times faster than A\b in MATLAB 7.3.
%
%   See also CS_QR, CS_AMD, CS_LUSOL, CS_CHOLSOL, MLDIVIDE.

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

error ('cs_qrsol mexFunction not found') ;


% Simulates an MFSK system using the PDFs of the signals which are inputs to
% the envelope detector

%Proakis p.307
%Uses RS encoding and decoding

clear
clc

%***********************
%*** MFSK Parameters ***
%***********************
m = 4
M = 2^m         % Amount of Symbols in the MFSK system

E_s = 1        % Signal Power in dB

%**************************
%*** RS code Parameters ***
%**************************

%m = 8
n = 2^m - 1
k = 3                   %Information symbols [ 1 - n-1] where n = 2^m - 1
h = n-k
t = h/2

%*** Generate the Galois Field ***

field = gftuple([-1:2^m-2]', m, 2);

%*** Generator Polynomial ***

%Lin + Costello, p.171

%Get the generator polynomial
c = [1 0];
p(1) = c(1);

for i = 1:h-1
    p(1) = gfmul(p(1),1,field);
    p(2) = 0;
    c = gfconv(c,p,field);
end
g = c;

%**************************

%*****************************
%*** Simulation parameters ***
%*****************************

% number of iterations
num_runs = 25

% the length of the data to be transmitted over the channel
num_data = 100       %Number of codewords to be transmitted (Increase to 10000 to obtain smoother curves)

length_data_n = n * num_data;

%***************************

SNR_dB = 2
inc = 0.5;

T_SER = [];
ED_UC_SER = [];
ED_C_SER = [];

SNR_arr = [];

for nr = 1:num_runs

    nr

    SNR_dB = SNR_dB + inc;

    %Clear the counters
    ACC_Stats_ED = 0;
    ACC_Stats_ED_DEC = 0;

    % for each transmitted codeword, do the following

    for num_codewords = 1:num_data

        %Create k random symbols to be encoded
        INFO = randint(1,k,[-1 M-2]);

        %Encode the INFO to form a RS Codeword
        RS_CODE = RS_ENC(INFO,n,k,g,field);

        %convert data to M_FSK data
        send_MFSK = ConvertRS2MFSK(RS_CODE);

        %***********************************

        %***************************
        %*** Do the demodulation ***
        %***************************

        Metric_table = MFSK_DEMOD(send_MFSK, M,E_s, SNR_dB); %Create a table of metrics for MFSK with specific SNR

        RECEIVED_ED = [];

        for j = 1:n

            Metrics = transpose(Metric_table(1:M,j));

            %--- Do for Envelope Detection ---
            RECEIVED_ED = [RECEIVED_ED DECODE_MFSK(Metrics,M)];

        end

        %*** End demodulation ***

        %**********************
        %*** Do RS decoding ***
        %**********************

        %*** Envelope detection ***

        RECEIVED_ED_SYMB = ConvertMFSK2RS(RECEIVED_ED);
        Stats_ED =  Compare(RECEIVED_ED_SYMB, RS_CODE,n,field);       %Without coding

        Stats_ED_DEC = Decode_and_compare(RECEIVED_ED,RS_CODE,n,k,t,h,g,field);

        %**********************

        %************************
        %*** Accumulute Stats ***
        %************************
        ACC_Stats_ED = ACC_Stats_ED + Stats_ED;
        ACC_Stats_ED_DEC = ACC_Stats_ED_DEC + Stats_ED_DEC;

        %************************

    end

    %***********************
    %*** Calculate Stats ***
    %***********************

    %*** Theoretical result for envelope detection ***
    prob_symb_err = prob_symb_error_MFSK(M,SNR_dB);

    %*** Envelope Detector Statistics ***
    sym_prob_ED = ACC_Stats_ED / length_data_n;      %Envelope demodulation without decoding
    dec_sym_prob_ED = ACC_Stats_ED_DEC/length_data_n;    %Envelope demodulation with decoding

    %*******************

    T_SER = [T_SER prob_symb_err];
    ED_UC_SER = [ED_UC_SER sym_prob_ED];
    ED_C_SER = [ED_C_SER dec_sym_prob_ED];

    SNR_arr = [SNR_arr SNR_dB];

end

%Plot the data
plot_compare = figure;
semilogy(SNR_arr,T_SER,'k*-' , SNR_arr, ED_UC_SER,'kd-' , ...
    SNR_arr, ED_C_SER,'kx-');

xlabel('SNR','FontWeight','Bold','FontSize',12);
ylabel('Probability of a symbol error','FontWeight','Bold','FontSize',12);
tt = [int2str(2^m) '-FSK (' int2str(n) ',' int2str(k) ') RS']
title(tt);
set(plot_compare,'Position',[10,10,1000,600]);

legend('Theoretical','Envelope detection - uncoded', 'Envelope detection - coded')


%%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%%

check_active_images;

% Reproject the patterns on the images, and compute the pixel errors:

ex = []; % Global error vector
x = []; % Detected corners on the image plane
y = []; % Reprojected points

if ~exist('alpha_c'),
   alpha_c = 0;
end;

for kk = 1:n_ima,

   eval(['omckk = omc_' num2str(kk) ';']);
   eval(['Tckk = Tc_' num2str(kk) ';']);

   if active_images(kk) & (~isnan(omckk(1,1))),

      %Rkk = rodrigues(omckk);

      eval(['y_' num2str(kk) '  = project_points2(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']);

      eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']);

      eval(['x_kk = x_' num2str(kk) ';']);

      eval(['ex = [ex ex_' num2str(kk) '];']);
      eval(['x = [x x_' num2str(kk) '];']);
      eval(['y = [y y_' num2str(kk) '];']);

   else

      %	eval(['y_' num2str(kk) '  = NaN*ones(2,1);']);

      % If inactivated image, the error does not make sense:
      eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']);

   end;

end;

err_std = std(ex')';


function [rms, nees] = compute_rms_nees_ave( rmsall, neesall )
% compute rms and nees from multiple tests

nsteps = size(rmsall.position, 2);
ntests = size(rmsall.position, 1);

% compute rms
rmspos_sum = sum(rmsall.position.^2, 1);
rms.position = sqrt(rmspos_sum/ntests);

rmsori_sum = sum(rmsall.orientation.^2, 1);
rms.orientation = sqrt(rmsori_sum/ntests);

% compute nees
neesall_pose = neesall.pose(:, 2:end);
neesall_orientation = neesall.orientation(:, 2:end);

neespose_sum = sum(neesall_pose, 1);
nees.pose = neespose_sum/ntests;

neesorientation_sum = sum(neesall_orientation, 1);
nees.orientation = neesorientation_sum/ntests;


function test_suite = test_conjugate_gradients
    clear all;
    initTestSuite;
end

function test_1
    A = [3 2; 2 6];
    X = [2 1 0 4; -2 2 0 4];
    B = A * X;
    solver = SPX_ConjugateDescent(A, B);
    x = solver.solve();
    %solver.printResults();
    verifyTrue(testCase, solver.hasConverged());
end

function test_2
    A  = [
    2 -1 0;
    -1 2 -1;
    0 -1 2];
    X = randi(10, 3,3);
    B = A * X;
    solver = SPX_ConjugateDescent(A, B);
    solver.MaxIterations = 50;
    x = solver.solve();
    verifyTrue(testCase, solver.hasConverged());
end


% PlotSphereIntensity(azimuth, elevation)
% PlotSphereIntensity(azimuth, elevation, intensity)
% h = PlotSphereIntensity(...)
%
% Plots the intensity (as color) of a number of points on a unit sphere.
% Input:
%   azimuth (phi), in degrees
%   elevation (theta), in degrees
%   intensity (optional, if not provided, a green sphere is produced)
%   All inputs must be vectors or matrices of the same size.
%   Data does not have to be evenly spaced. When there aren't enough points
%   to draw a smooth sphere, additional points (with color) are
%   interpolated.
% Output:
%   h - a handle to the patch object
%
% The axes are also plotted:
%   positive x axis is red
%   positive y axis is green
%   positive z axis is blue
%
% Author: David Johnstone (DSTO, WSD, Australia)
%         david.johnstone@dsto.defence.gov.au
%         Ninh Duong (DSTO, WSD, Australia)
%         ninh.duong@dsto.defence.gov.au
% Date: 18 March, 2008

function varargout = PlotSphereIntensity(az, el, varargin)

error(nargchk(2, 3, nargin))
error(nargoutchk(0, 1, nargout))

if (any(size(az) ~= size(el)) || (nargin == 3 && any(size(az) ~= size(varargin{1}))))
    error('input data has inconsistent sizes')
end

az = az(:) * pi / 180;
el = el(:) * pi / 180;

if (nargin == 3)
    c = varargin{1};
    c = c(:);
else
    c = ones(length(az),1);
end

% Construct cartesian points (x, y, z) from the given azimuth and elevation
% data, so that each azimuth elevation data pair are a point on the surface
% of a unit sphere.
x = cos(el) .* cos(az);
y = cos(el) .* sin(az);
z = sin(el);

[x y z c] = Sphericalise(x, y, z, c);

% Create a list of facets.
K = convhulln([x y z]);

% Plot the data
h = trisurf(K, x, y, z, c);
shading interp
axis equal
view([1 1 1])

plotSetting = get(gca, 'NextPlot');
hold on

xAxis = [0 0 0; 10 0 0];
yAxis = [0 0 0; 0 10 0];
zAxis = [0 0 0; 0 0 10];
plotAxis(xAxis, 'r', 'LineWidth', 2)
plotAxis(yAxis, 'g', 'LineWidth', 2)
plotAxis(zAxis, 'b', 'LineWidth', 2)

set(gca, 'NextPlot', plotSetting);

text(1.4,0,0.04,'x', 'FontSize', 16)
text(0,1.4,0.04,'y', 'FontSize', 16)
text(0.04,0.04,1.4,'z', 'FontSize', 16)

if (nargout == 1)
    varargout{1} = h;
end

end

function plotAxis(axis, varargin)
plot3(axis(:,1), axis(:,2), axis(:,3), varargin{:})
end

% [x2 y2 z2 C2] = Sphericalise(x, y, z, C)
%
% Creates a unit sphere with color data linearly interpolated across it
% based on a given set of points.
% Input:
%    x, y and z are equal sized vectors/matrices and describe the
%        points on a sphere.
%    C is the color data, and is the same size as x, y and z.
%    x, y, z and C must all be the same size. They are used as nx1
%        matrices with x(1), y(1), z(1) describing the location of the
%        first point, and C(1) containing the color for it.
% Output:
%   x, y, z and color so that the data now looks like a unit sphere.

% Method:
%   This algorithm works by triangulating the data and splitting any lines
%   that are longer than a certain threshold (chosen as pi/10 as this gives
%   a fairly smooth sphere.) convhulln is used to triangulate the surface
%   of the sphere, and the indices it returns are used to identify the
%   start and end points of the lines. If the line across the surface of
%   the sphere is too long, a new point is created halfway between the
%   endpoints, and it is scaled to have a magnitude of 1 so that it will
%   lie on the surface of the unit sphere. After all the lines have been
%   checked (and split if necessary), this process is repeated if any lines
%   were split. This repeating must be done as the triangulation will
%   introduce new line segments with the new point, and these may be too
%   long.

function [x2 y2 z2 C2] = Sphericalise(x, y, z, C)

splitLine = 1; % set to 1 initially as we want it to do the loop at least
% once, and MATLAB doesn't have a do while loop

x2 = x(:);
y2 = y(:);
z2 = z(:);
C2 = C(:);

while splitLine
    T = convhulln([x2 y2 z2]);

    % It is best to split lines on a per line basis (and not a per triangle
    % basis) as each line is shared by two triangles, so any line that is to be
    % split will be split twice (and the new point will be in the same place)
    lines = unique([T(:,1) T(:,2); T(:,1) T(:,3); T(:,2) T(:,3)], 'rows');

    splitLine = 0;
    for line = lines'

        A = struct('x', x2(line(1)), 'y', y2(line(1)), 'z', z2(line(1)), 'C', C2(line(1)));
        B = struct('x', x2(line(2)), 'y', y2(line(2)), 'z', z2(line(2)), 'C', C2(line(2)));

        % if the length of the geodesic is longer than pi/10
        if acos(dot2(A, B)) > pi/10
            splitLine = 1;

            % add point between A and B

            % It is possible to calculate elevation and azimuth for A and
            % B, and then interpolate this to get elevation and azimuth for
            % P, and finally convert these back to cartesian coordinates,
            % but this 1) is inefficient and 2) doesn't work around the
            % poles (at least if theta and phi are linearly interpolated).

            P.x = (A.x + B.x) / 2;
            P.y = (A.y + B.y) / 2;
            P.z = (A.z + B.z) / 2;
            magnitude = sqrt(P.x^2 + P.y^2 + P.z^2);
            P.x = P.x / magnitude;
            P.y = P.y / magnitude;
            P.z = P.z / magnitude;

            P.C = (A.C + B.C) / 2;

            x2(end+1) = P.x;
            y2(end+1) = P.y;
            z2(end+1) = P.z;
            C2(end+1) = P.C;

        end
    end
end

end

function d = dot2(A, B)
d = dot([A.x A.y A.z], [B.x B.y B.z]);
end


% Analysis of the Gradient of Convolution
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     01/03/2020
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

figureIdx           = 0; %<! Continue from Question 1
figureCounterSpec   = '%04d';

generateFigures = ON;

CONVOLUTION_SHAPE_FULL         = 1;
CONVOLUTION_SHAPE_SAME         = 2;
CONVOLUTION_SHAPE_VALID        = 3;

DIFF_MODE_FORWARD   = 1;
DIFF_MODE_BACKWARD  = 2;
DIFF_MODE_CENTRAL   = 3;
DIFF_MODE_COMPLEX   = 4;

%% Simulation Parameters

numCoefficients = 1;
numSamples      = 14;

diffMode    = DIFF_MODE_CENTRAL;
epsVal      = 1e-6;

%% Generate Data

vH = rand(numCoefficients, 1);
vX = randn(numSamples, 1);

vY = conv2(vX, vH, 'same');

hObjFun = @(vX) 0.5 * sum((conv2(vX, vH, 'same') - vY) .^ 2);

%% Numerical Derivative
vX0 = randn(numSamples, 1);

vDRef = CalcFunGrad(vX0, hObjFun, diffMode, epsVal);
vDD = conv2(vH(end:-1:1), (conv2(vX0, vH, 'same') - vY), 'full');

firstIdx = floor((numSamples + numCoefficients - 1 - numSamples) / 2) + 1;
lastIdx = firstIdx + numSamples - 1;
vD = vDD(firstIdx:lastIdx);

max(abs(vDRef - vD))

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function w = ymdf_to_weekday_islamic_a ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_WEEKDAY_ISLAMIC_A returns the weekday of an Islamic A YMDF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, integer W, is the week day number of the date, with
%    1 for Sunday, through 7 for Saturday.
%
  jed = ymdf_to_jed_islamic_a ( y, m, d, f );

  [ w, f2 ] = jed_to_weekday ( jed );

  return
end


%TESTP Error estimation of Parzen classifier
%
% 	E = TESTP(A,H,T)
% 	E = TESTP(A,H)
%
% INPUT
%     A    input dataset
%     H    matrix smoothing parameters (optional, def: determined via
%            parzenc)
%     T    test dataset (optional)
%
% OUTPUT
%     E    estimated error rate
%
% DESCRIPTION
% Tests a dataset T on dataset A using a Parzen classification and returns
% the classification error E.  Returns the leave-one-out error estimate. If
% H is not given, it is determined by PARZENC.
%
% SEE ALSO (PRTools Guide)
% DATASETS, MAPPINGS, PARZEN_MAP, PARZENML, PARZENC.

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% may be to be merged with parzen_map, see also testk

% $Id: testp.m,v 1.4 2009/09/08 21:27:51 duin Exp $

function [e,d] = testp(a,h,t)

	isvaldfile(a,2,2);
	a = testdatasize(a);
	a = testdatasize(a,'objects');

	if nargin < 2, [W,h] = parzenc(a); end
	[m,k,c] = getsize(a);
	nlab = getnlab(a);
	lablist = getlablist(a);
	p = getprior(a);

	if length(h) == 1, h = h*ones(1,c); end
	if length(h) ~= c, error('Wrong number of smoothing parameters'); end

	% if no test dataset is specified
	if nargin <= 2
		% find for each sample cross-validated estimate
		% of aposteriori probability.
		d = classp(a,nlab,h,p);
		[dmax,J] = max(d',[],1);
		e = nlabcmp(lablist(J,:),lablist(nlab,:)) / m;
        % if the validation dataset is given
	elseif nargin == 3
		lablistt = getlablist(t);
		[n,kt] = size(t);
		nlabt = getnlab(t);
		if k ~= kt
			error('Data sizes do not match');
		end
		d = classp(a,nlab,h,p,t);
		[dmax,J] = max(d',[],1);
		e = nlabcmp(lablistt(J,:),lablistt(nlabt,:)) / n;
	end
return

%CLASSP estimate of Parzen density (if t is not specified,
% a leave-one-out error estimate for a is returned)
function F = classp(a,nlab,h,p,t)

	[m,k] = size(a);
	maxa = max(max(abs(a)));
	a = a/maxa;
	h = h/maxa;
	if nargin < 5
		mt = m;
	else
		[mt,kt] = size(t);
		t = t/maxa;
	end

	c = max(nlab);
	alf=sqrt(2*pi)^k; % density normalization factor
	[num,n] = prmem(mt,m); % use batches to avoid excessive memory usage
	F = ones(mt,c);
  s = sprintf('Compute distance matrix in %i batches: ',num);
  prwaitbar(num,s);
	for i = 0:num-1
    prwaitbar(num,i+1,[s int2str(i+1)]);
		if i == num-1
			nn = mt - num*n + n;
		else
			nn = n;
		end
		range = [i*n+1:i*n+nn];
		if nargin <= 4
			D = +distm(a,a(range,:));
			D(i*n+1:m+1:i*n+nn*m) = inf*ones(1,nn); % set distances to itself at inf
		else
			D = +distm(a,t(range,:));
		end
		for i=1:c
			I = find(nlab == i);
			if length(I) > 0
				F(range,i) = p(i).*sum(exp(-D(I,:)*0.5./(h(i).^2)),1)'./(length(I)*alf*h(i)^k);
			end
		end
  end
  prwaitbar(0);
	F = F + realmin;
	F = F ./ (sum(F')'*ones(1,c));

return


function r8utp_print_some ( n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% R8UTP_PRINT_SOME prints some of an R8UTP matrix.
%
%  Discussion:
%
%    The R8UTP storage format is appropriate for an upper triangular
%    matrix.  Only the upper triangle of the matrix is stored,
%    by successive partial columns, in an array of length (N*(N+1))/2,
%    which contains (A11,A12,A22,A13,A23,A33,A14,...,ANN)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, real A((N*(N+1))/2), the matrix.
%
%    Input, integer ILO, JLO, IHI, JHI, the first row and
%    column, and the last row and column to be printed.
%
%    Input, string TITLE, a title.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  incx = 5;
%
%  Print the columns of the matrix, in strips of 5.
%
  for j2lo = jlo : incx : jhi

    j2hi = j2lo + incx - 1;
    j2hi = min ( j2hi, n );
    j2hi = min ( j2hi, jhi );

    inc = j2hi + 1 - j2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col: ' );

    for j = j2lo : j2hi
      j2 = j + 1 - j2lo;
      fprintf ( 1, '%7d       ', j );
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row\n' );
    fprintf ( 1, '  ---\n' );
%
%  Determine the range of the rows in this strip.
%
    i2lo = max ( ilo, 1 );
    i2hi = min ( ihi, n );

    for i = i2lo : i2hi

      fprintf ( 1, '%4d  ', i );
%
%  Print out (up to) 5 entries in row I, that lie in the current strip.
%
      for j2 = 1 : inc

        j = j2lo - 1 + j2;

        if ( i <= j )
          aij = a(i+(j*(j-1))/2);
        else
          aij = 0.0;
        end

        fprintf ( 1, '%12g  ', aij );

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


%--------------------------------------------------------------------------
%   [dataout] = d2b(datain,N_bit)
%--------------------------------------------------------------------------
%   功能
%   转换10进制有符号数为二进制格式(常用于FPGA验证)
%--------------------------------------------------------------------------
%   输入:
%           datain                  10进制数
%           N_bit                   转换位数
%   output:
%           dataout                 二进制数
%--------------------------------------------------------------------------
%   例子:
%   d2b(13,5)
%   ans =
%       "1101"
%   d2b(-13,5)
%   ans =
%       "10011"
%--------------------------------------------------------------------------
function dataout = d2b(datain,N_bit)
[X,Y,Z] = size(datain);
datain(datain < 0) = datain(datain < 0)+2^N_bit;
h = dec2bin(datain);
for index = 1:X*Y*Z
    temp(index)= string(h(index,:));
end
dataout = reshape(temp,[X Y Z]);
end


function test_failed=test_dgt
%TEST_DGT  Test DGT
%
%  This script runs a throrough test of the DGT routine,
%  testing it on a range of input parameters.
%
%  The computational backend is tested this way, but the
%  interface is not.
%
%  The script tests dgt, idgt, gabdual and gabtight.
%
%  Use TEST_WFAC and TEST_DGT_FAC for more specific testing
%  of the DGT backend.

Lr=[24,16,144,108,144,24,135,35,77,20];
ar=[ 4, 4,  9,  9, 12, 6,  9, 5, 7, 1];
Mr=[ 6, 8, 16, 12, 24, 8,  9, 7,11,20];

test_failed=0;

disp(' ===============  TEST_DGT ================');

disp('--- Used subroutines ---');

which comp_wfac
which comp_iwfac
which comp_sepdgt
which comp_isepdgt
which comp_sepdgtreal
which comp_isepdgtreal
which comp_gabdual_long
which comp_gabtight_long

for ii=1:length(Lr);

  L=Lr(ii);

  M=Mr(ii);
  a=ar(ii);

  b=L/M;
  N=L/a;
  c=gcd(a,M);
  d=gcd(b,N);
  p=a/c;
  q=M/c;

  for rtype=1:2

    if rtype==1
      rname='REAL ';
      g=tester_rand(L,1);
    else
      rname='CMPLX';
      g=tester_crand(L,1);
    end;

    global LTFAT_TEST_TYPE;
    if strcmpi(LTFAT_TEST_TYPE,'single')
        C = gabframebounds(g,a,M);
        while C>1e3
%             warning(sprintf(['The frame is too badly conditioned '...
%                              'for single precision. Cond. num. %d. '...
%                              ' Trying again.'],C));

                         if rtype==1
                             rname='REAL ';
                             g=tester_rand(L,1);
                         else
                             rname='CMPLX';
                             g=tester_crand(L,1);
                         end;
                         C = gabframebounds(g,a,M);
        end
    end

    gd=gabdual(g,a,M);
    gt=gabtight(g,a,M);

    % --- Test windows against their reference implementations. ---

    ref_gd=ref_gabdual(g,a,M);
    res=norm(ref_gd-gd);
    [test_failed,fail]=ltfatdiditfail(res,test_failed);
    fprintf(['REFDUAL %s L:%3i a:%3i b:%3i c:%3i d:%3i p:%3i q:%3i '...
               '%0.5g %s\n'],rname,L,a,b,c,d,p,q,res,fail);

    ref_gt=ref_gabtight(g,a,M);
    res=norm(ref_gt-gt);
    [test_failed,fail]=ltfatdiditfail(res,test_failed);
    s=sprintf(['REFTIGHT %s L:%3i a:%3i b:%3i c:%3i d:%3i p:%3i q:%3i '...
               '%0.5g %s'],rname,L,a,b,c,d,p,q,res,fail);
    disp(s);

    % ---- Test gabdualnorm --------------------------------------
    res=gabdualnorm(g,gd,a,M);
    [test_failed,fail]=ltfatdiditfail(res,test_failed);
    s=sprintf(['DUALNORM1 %s L:%3i a:%3i b:%3i c:%3i d:%3i p:%3i q:%3i '...
               '%0.5g %s'],rname,L,a,b,c,d,p,q,res,fail);
    disp(s);

    [o1,o2]=gabdualnorm(g,gd,a,M);
    res=o1-1+o2;
    [test_failed,fail]=ltfatdiditfail(res,test_failed);
    s=sprintf(['DUALNORM2 %s L:%3i a:%3i b:%3i c:%3i d:%3i p:%3i q:%3i '...
               '%0.5g %s'],rname,L,a,b,c,d,p,q,res,fail);
    disp(s);

    for W=1:3

      if rtype==1
        f=tester_rand(L,W);
      else
        f=tester_crand(L,W);
      end;

      % --- Test DGT against its reference implementation. ---

      cc=dgt(f,g,a,M);
      cc2=ref_dgt(f,g,a,M);

      cdiff=cc-cast(cc2,class(cc));
      res=norm(cdiff(:));
      [test_failed,fail]=ltfatdiditfail(res,test_failed);
      s=sprintf(['REF %s L:%3i W:%2i a:%3i b:%3i c:%3i d:%3i p:%3i q:%3i '...
                 '%0.5g %s'],rname,L,W,a,b,c,d,p,q,res,fail);
      disp(s)

      % --- Test reconstruction of IDGT using a canonical dual window. ---

      r=idgt(cc,gd,a);
      res=norm(f-r,'fro');
      [test_failed,fail]=ltfatdiditfail(res,test_failed);
      s=sprintf(['REC %s L:%3i W:%2i a:%3i b:%3i c:%3i d:%3i p:%3i q:%3i ' ...
                 '%0.5g %s'],rname,L,W,a,b,c,d,p,q,res,fail);
      disp(s)

      % --- Test reconstruction of IDGT using a canonical tight window. ---

      res=norm(f-idgt(dgt(f,gt,a,M),gt,a),'fro');
      [test_failed,fail]=ltfatdiditfail(res,test_failed);
      s=sprintf(['TIG %s L:%3i W:%2i a:%3i b:%3i c:%3i d:%3i p:%3i q:%3i ' ...
                 '%0.5g %s'],rname,L,W,a,b,c,d,p,q,res,fail);
      disp(s);

      % Test the real valued transform
      if rtype==1

        % --- Reference test ---
        ccreal=dgtreal(f,g,a,M);
        M2=floor(M/2)+1;

        cdiff=cc(1:M2,:,:)-ccreal;
        res=norm(cdiff(:));
        [test_failed,fail]=ltfatdiditfail(res,test_failed);
        s=sprintf(['REFREAL   L:%3i W:%2i a:%3i b:%3i c:%3i d:%3i p:%3i ' ...
                   'q:%3i %0.5g %s'],L,W,a,b,c,d,p,q,res,fail);
        disp(s);

        % --- Reconstruction test ---

        rreal=idgtreal(ccreal,gd,a,M);

        res=norm(f-rreal,'fro');
        [test_failed,fail]=ltfatdiditfail(res,test_failed);
        s=sprintf(['RECREAL   L:%3i W:%2i a:%3i b:%3i c:%3i d:%3i p:%3i ' ...
                   'q:%3i %0.5g %s'],L,W,a,b,c,d,p,q,res,fail);
        disp(s)

      end;
    end;

  end;

end;


function M = fixedrankMNquotientfactory(m, n, k)
% Manifold of m-by-n matrices of rank k with quotient geometry.
%
% function M = fixedrankMNquotientfactory(m, n, k)
%
% This follows the quotient geometry described in the following paper:
% P.-A. Absil, L. Amodei and G. Meyer,
% "Two Newton methods on the manifold of fixed-rank matrices endowed
%  with Riemannian quotient geometries", arXiv, 2012.
%
% Paper link: http://arxiv.org/abs/1209.0068
%
% A point X on the manifold is represented as a structure with two
% fields: M and N. The matrix M (mxk) is orthonormal, while the matrix N
% (nxk) is full-rank.
%
% Tangent vectors are represented as a structure with two fields (M, N).

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, Dec. 30, 2012.
% Contributors:
% Change log:

M.name = @() sprintf('MN'' quotient manifold of %dx%d matrices of rank %d', m, n, k);

M.dim = @() (m+n-k)*k;

% Choice of the metric is motivated by the symmetry present in the
% space
M.inner = @(X, eta, zeta) eta.M(:).'*zeta.M(:) + eta.N(:).'*zeta.N(:);

M.norm = @(X, eta) sqrt(M.inner(X, eta, eta));

M.dist = @(x, y) error('fixedrankMNquotientfactory.dist not implemented yet.');

M.typicaldist = @() 10*k;

symm = @(X) .5*(X+X');
stiefel_proj = @(M, H) H - M*symm(M'*H);

M.egrad2rgrad = @egrad2rgrad;
    function eta = egrad2rgrad(X, eta)
        eta.M = stiefel_proj(X.M, eta.M);
    end

M.ehess2rhess = @ehess2rhess;
    function Hess = ehess2rhess(X, egrad, ehess, eta)

        % Directional derivative of the Riemannian gradient
        Hess.M = ehess.M - eta.M*symm(X.M'*egrad.M);
        Hess.M = stiefel_proj(X.M, Hess.M);

        Hess.N = ehess.N;

        % Projection onto the horizontal space
        Hess = M.proj(X, Hess);
    end

M.proj = @projection;
    function etaproj = projection(X, eta)

        % Start by projecting the vector from Rmp x Rnp to the tangent
        % space to the total space, that is, eta.M should be in the
        % tangent space to Stiefel at X.M and eta.N is arbitrary.
        eta.M = stiefel_proj(X.M, eta.M);

        % Now project from the tangent space to the horizontal space, that
        % is, take care of the quotient.

        % First solve a Sylvester equation (A symm., B skew-symm.)
        A = X.N'*X.N + eye(k);
        B = eta.M'*X.M + eta.N'*X.N;
        B = B-B';
        omega = lyap(A, -B);

        % And project along the vertical space to the horizontal space.
        etaproj.M = eta.M + X.M*omega;
        etaproj.N = eta.N + X.N*omega;

    end

M.exp = @exponential;
    function Y = exponential(X, eta, t)
        if nargin < 3
            t = 1.0;
        end

        A = t*X.M'*eta.M;
        S = t^2*eta.M'*eta.M;
        Y.M = [X.M t*eta.M]*expm([A -S ; eye(k) A])*eye(2*k, k)*expm(-A);

        % re-orthonormalize (seems necessary from time to time)
        [Q R] = qr(Y.M, 0);
        Y.M = Q * diag(sign(diag(R)));

        Y.N = X.N + t*eta.N;

    end

% Factor M lives on the Stiefel manifold, hence we will reuse its
% random generator.
stiefelm = stiefelfactory(m, k);

M.retr = @retraction;
    function Y = retraction(X, eta, t)
        if nargin < 3
            t = 1.0;
        end

        Y.M = uf(X.M + t*eta.M); % This is a valid retraction
        Y.N = X.N + t*eta.N;
    end

M.hash = @(X) ['z' hashmd5([X.M(:) ; X.N(:)])];

M.rand = @random;
    function X = random()
        X.M = stiefelm.rand();
        X.N = randn(n, k);
    end

M.randvec = @randomvec;
    function eta = randomvec(X)
        eta.M = randn(m, k);
        eta.N = randn(n, k);
        eta = projection(X, eta);
        nrm = M.norm(X, eta);
        eta.M = eta.M / nrm;
        eta.N = eta.N / nrm;
    end

M.lincomb = @lincomb;

M.zerovec = @(X) struct('M', zeros(m, k), 'N', zeros(n, k));

M.transp = @(x1, x2, d) projection(x2, d);

end

% Linear combination of tangent vectors
function d = lincomb(x, a1, d1, a2, d2) %#ok<INMSL>

if nargin == 3
    d.M = a1*d1.M;
    d.N = a1*d1.N;
elseif nargin == 5
    d.M = a1*d1.M + a2*d2.M;
    d.N = a1*d1.N + a2*d2.N;
else
    error('Bad use of fixedrankMNquotientfactory.lincomb.');
end

end

function A = uf(A)
[L, unused, R] = svd(A, 0);
A = L*R';
end


function [model] = linearsvm(X, Y, C, dual)
  % [model] = linearsvm(X, Y, C, dualparam);
  % Build linear svm using liblinear package
  % X: the instance X feature sparse matrix
  % Y: the label Y each column is  a label, the negative class could be -1 or 0
  % C: the trade-off parameter of SVM
  % dual: whether or not use primal or dual solver (optional)
  %
  %  model: the constructed SVM classifier
  %  to obtain prediction score of test set X of n test instances
  %      prediction_score = X * model.W + repmat(model.bias, n, 1)

  if nargin < 4
	dual = 0;
  end

  param.alpha = C;
  %C = param.alpha;
  [n, T] = size(Y);
  [n, d] = size(X);

%   if issparse(Y)
%     Y = full(Y);
%   end
  if ~issparse(X)
      X = sparse(X);
  end

  %Y(Y==0)=-1;
  if dual==0
  paramstr = sprintf('-s 2 -c %f -B 1 -q', param.alpha)
  else
  paramstr = sprintf('-s 1 -c %f -B 1 -q', param.alpha)
  end

%% ===PLEASE uncomment the following code if you use liblinear <1.5 ======
%   if dual==0
%   	paramstr = sprintf('-s 2 -c %f', param.alpha)
%   else
%   	paramstr = sprintf('-s 1 -c %f', param.alpha)
%   end
%% =======================================================================

  W = zeros(d,T);
  bias = zeros(1,T);

  for t=1:T
    t
    y = -1*ones(n, 1);
    y(Y(:, t)==1)=1;
    mod_ind = train(y, X, paramstr);
    % fprintf('the size of vector w is %d: %d\n', length(mod_ind.w), d);
    W(:, t) = mod_ind.w(1:d);
    bias(t) = mod_ind.w(end);
    if mod_ind.Label(1)==-1
       W(:,t)= -W(:,t);
       bias(t) = -bias(t);
    end
  end
  model.W = W;
  model.bias = bias;
  model.method = 'svm';
  model.param = param;


function ldldemo
%LDLDEMO demo program for LDL
%
% Example:
%   ldldemo
%
% See also ldlsparse.

% Copyright 2006-2007 by Timothy A. Davis, Univ. of Florida

% compile the LDLSPARSE and LDLSYMBOL mexFunctions
help ldlsparse

fprintf ('\nTesting ldlsparse and ldlsymbol:\n') ;

% create a small random symmetric positive definite sparse matrix
n = 100 ;
d = 0.03 ;
rand ('state', 0) ;
randn ('state', 0) ;
A = sprandn (n, n, d) ;
A = speye (n) + A*A' ;
b = randn (n, 1) ;

figure (1)
clf
subplot (2,2,1) ;
spy (A) ;
title ('original matrix') ;

% permute for sparsity
p = symamd (A) ;
C = A (p,p) ;

subplot (2,2,2) ;
spy (C) ;
title ('permuted matrix') ;
drawnow

% factorize, without using ldlsparse's internal permutation
[L, D, Parent, fl] = ldlsparse (C) ;
L = L + speye (n) ;
err = norm (L*D*L' - C, 1) ;
fprintf ('norm (LDL''-PAP'') = %g\n', err) ;

% solve Ax=b
x = L' \ (D \ (L \ (b (p)))) ;
x (p) = x ;
resid = norm (A*x-b) ;
fprintf ('residual %g for ldlsparse, flops %10.1f\n', resid, fl) ;

% solve Ax=b with one call to ldlsparse
x = ldlsparse (C, [ ], b (p)) ;
x (p) = x ;
resid = norm (A*x-b) ;
fprintf ('residual %g for ldlsparse solve\n', resid) ;

subplot (2,2,3) ;
spy (L + D + L') ;
title ('L+D+L''') ;

subplot (2,2,4) ;
treeplot (Parent)
title ('elimination tree') ;

% try ldlrow (this will be slow)
[L, D] = ldlrow (C) ;
x = L' \ (D \ (L \ (b (p)))) ;
x (p) = x ;
resid = norm (A*x-b) ;
fprintf ('residual %g for ldlrow.m\n', resid) ;

% factorize, using ldlsparse's internal permutation
[L, D, Parent, fl] = ldlsparse (A, p) ;
L = L + speye (n) ;
err = norm (L*D*L' - C, 1) ;
fprintf ('norm (LDL''-PAP'') = %g\n', err) ;

% solve Ax=b
x = L' \ (D \ (L \ (b (p)))) ;
x (p) = x ;
resid = norm (A*x-b) ;
fprintf ('residual %g for ldlsparse, flops %10.1f\n', resid, fl) ;

% solve Ax=b with one call to ldlsparse
x = ldlsparse (A, p, b) ;
resid = norm (A*x-b) ;
fprintf ('residual %g for ldlsparse solve\n\n', resid) ;

% compare ldlsymbol and symbfact
[Lnz, Parent, fl] = ldlsymbol (A) ;
fprintf ('Original matrix: nz in L: %5d  flop count: %g\n', sum (Lnz), fl) ;

Lnz2 = symbfact (A) - 1 ;
Parent2 = etree (A) ;
fl2 = sum (Lnz2 .* (Lnz2 + 2)) ;
if (any (Lnz ~= Lnz2))
    error ('Lnz mismatch') ;
end
if (any (Parent ~= Parent2))
    error ('Parent mismatch') ;
end
if (fl ~= fl2)
    error ('fl mismatch') ;
end

[Lnz, Parent, fl] = ldlsymbol (A, p) ;
fprintf ('Permuted matrix: nz in L: %5d  flop count: %g\n', sum (Lnz), fl) ;

Lnz2 = symbfact (A (p,p)) - 1 ;
Parent2 = etree (A (p,p)) ;
fl2 = sum (Lnz2 .* (Lnz2 + 2)) ;
if (any (Lnz ~= Lnz2))
    error ('Lnz mismatch') ;
end
if (any (Parent ~= Parent2))
    error ('Parent mismatch') ;
end
if (fl ~= fl2)
    error ('fl mismatch') ;
end

fprintf ('\nldldemo: all tests passed\n') ;


%IM_MINF Fixed mapping for minimum filter (DIP_Image) (DIP_Image)
%
%	B = IM_MINF(A,SIZE,SHAPE)
%	B = A*IM_MINF([],SIZE,SHAPE)
%	B = A*IM_MINF(SIZE,SHAPE)
%
% INPUT
%   A        Dataset with object images dataset (possibly multi-band)
%   SIZE     Filter width in pixels, default SIZE = 7
%   SHAPE    String with shape:'rectangular', 'elliptic', 'diamond'
%            Default: elliptic
%
% OUTPUT
%   B        Dataset with filtered images
%
% SEE ALSO (PRTools Guide)
% DATASETS, DATAFILES, DIP_IMAGE, MINF

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

function b = im_minf(varargin)

	argin = shiftargin(varargin,'scalar');
  argin = setdefaults(argin,[],7,'elliptic');
  if mapping_task(argin,'definition')
    b = define_mapping(argin,'fixed');
    b = setname(b,'Minimum filter');
  else
    [a,size,shape] = deal(argin{:});
    if isa(a,'prdataset') % allows datafiles too
      isobjim(a);
      b = filtim(a,mfilename,{size,shape});
    elseif isa(a,'double') || isa(a,'dip_image') % here we have a single image
      if checktoolbox('dipimage')
        a = 1.0*dip_image(a);
        b = minf(a,size,shape);
      else
        diplibwarn
        %prwarning(1,'Rectangular shape only')
        shape = ones(size,size);
        b = ordfilt2(a,1,shape);
      end
    end
  end

return


function [rotDetector,angles]=coord2angles(geo,det_origin,up_vec,norm_vec)
%COORD2ANGLES converts real world coordinates of the detector to TIGRE angles
%
%
% The detector origin is assumed to have no detector offset!
%
% There are 2 relevant rotations here. The ones around the detector center
% and the ones around the object center. From the origin and 2 vectors we
% can obtain both.

% Input checking:

assert(((geo.DSD-geo.DSO)-sqrt(sum(det_origin(:).^2))<1e-4),'Detector is not at geo.DOD distance from origin.');

assert(dot(up_vec,norm_vec)<1e-4,'Up vector and normal vector are not orthonormal');

% Lets normalize the vectors
up_vec=up_vec/sqrt(sum(up_vec(:).^2));
norm_vec=norm_vec/sqrt(sum(norm_vec(:).^2));

%% first lets obtain local rotation (around detector origin)
% Lets define the vectors we want

non_rotated_norm_vec=-det_origin/sqrt(sum(det_origin(:).^2));

% The up vector is harder, as there are infinte posibilities. We chose the
% one that is angularly closer to the real one. This is done by projecting
% the vector onto the plane
% https://math.stackexchange.com/questions/633181/formula-to-project-a-vector-onto-a-plane

non_rotated_up_vec=up_vec-(dot(up_vec,non_rotated_norm_vec)*non_rotated_norm_vec);

% solve! (Wahba's problem using SVD)

B=up_vec*non_rotated_up_vec.'+norm_vec*non_rotated_norm_vec.'; % this should be 3x3
assert(isequal(size(B),[3 3]),'input vectors are not 3x1, (they are likely 1x3)')

[U,~,V]=svd(B);
M=eye(3); M(3,3)=det(U)*det(V);
R=U*M*V';

% This rotation is roll-pitch-yaw.
% Computing Euler angles from a rotation matrix -Greg Slabaugh

if (abs(R(3,1))-1) < 1e-10
    pitch1=-asin(R(3,1));
    % pitch2=pi-pitch1;
    yaw1=atan2(R(3,2)/cos(pitch1),R(3,3)/cos(pitch1));
    % yaw2=atan2(R(3,2)/cos(pitch2),R(3,3)/cos(pitch2));
    roll1=atan2(R(2,1)/cos(pitch1),R(1,1)/cos(pitch1));
    % roll2=atan2(R(2,1)/cos(pitch2),R(1,1)/cos(pitch2));
else
    roll1=0;
    if sign(R(3,1))==1
        pitch1=-pi/2;
        yaw1=-roll1+atan2(-R(1,2),-R(1,3));
    else
        pitch1=pi/2;
        yaw1=roll1+atan2(R(1,2),R(1,3));
    end
end

rotDetector=[roll1;pitch1;yaw1];

%% Now lets get global rotation around the origin. We need new vectors

origin_norm_vec=det_origin+non_rotated_norm_vec;
origin_up_vec=det_origin+non_rotated_up_vec;

% now we can reuse the variables
non_rotated_norm_vec=[geo.DSO-1;0;0];
non_rotated_up_vec=[geo.DSO;0;1];

B=origin_up_vec*non_rotated_up_vec.'+origin_norm_vec*non_rotated_norm_vec.'; % this should be 3x3
[U,~,V]=svd(B);
M=eye(3); M(3,3)=det(U)*det(V);
R=U*M*V';

if (abs(R(3,3))-1) < 1e-10

    angle21=acos(R(3,3));
    angle22=angle21+pi;
    angle31=atan2(R(3,2)/sin(angle21),-R(3,1)/sin(angle21));
    angle32=atan2(R(3,2)/sin(angle22),-R(3,1)/sin(angle22));
    angle11=atan2(R(2,3)/sin(angle21),R(1,3)/sin(angle21));
    angle12=atan2(R(2,3)/sin(angle22),R(1,3)/sin(angle22));
else
    angle31=0;
    if sign(R(3,3))==1
        angle21=pi/2;
        angle11=-angle31+atan2(R(1,1),R(2,1));
    else
        angle21=-pi/2;
        angle21=pi/2;
        angle11=angle31+atan2(-R(1,1),R(2,1));
    end
end
angles=[angle11;angle21;angle31];


function c = tanh(a)
% TANH for adiff objects.

c = adiff( tanh(a.x), rowmult(1./cosh(a.x).^2, a.dx), a.root);


function x=randcorr(n,R)
% RANDCORR Generates corremlated random variables
% Generates n vector valued variates with uniform marginals and correlation
% matrix R.
% Returns an nxk matrix, where k is the order of R.
  k=size(R,1);
  R=2*sin((pi/6)*R);
  x=normcdf(randn(n,k)*chol(R));


function [ p, t ] = cvt_triangle_uniform ( n, sample_num, delaunay_display )

%*****************************************************************************80
%
%% CVT_TRIANGLE_UNIFORM demonstrates how a CVT can be computed and displayed in MATLAB.
%
%  Discussion:
%
%    This simple example carries out an iterative CVT calculation in a
%    triangle, with a uniform density.  The initial placement of the
%    generators is random.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Qiang Du, Vance Faber, Max Gunzburger,
%    Centroidal Voronoi Tessellations: Applications and Algorithms,
%    SIAM Review,
%    Volume 41, 1999, pages 637-676.
%
%  Parameters:
%
%    Input, integer N, the number of generators.
%
%    Input, integer SAMPLE_NUM, the number of sample points.
%
%    Input, logical DELAUNAY_DISPLAY, is TRUE (nonzero) if the Delaunay
%    triangulation is to be displayed.
%
%    Output, real P(N,2), the location of the generators.
%
%    Output, integer T(NT,3), information defining the Delaunay
%    triangulation of the generators.  NT is the number of triangles,
%    which varies depending on the arrangement of the generators.
%

%
%  Here's the triangle we will use.
%
  triangle = [ 0.50, 1.00; ...
               0.00, 0.75; ...
               0.95, 0.00 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_TRIANGLE_UNIFORM:\n' );
  fprintf ( 1, '  A simple demonstration of a CVT computation\n' );
  fprintf ( 1, '  (Centroidal Voronoi Tessellation)\n' );
  fprintf ( 1, '  in a triangle, with a uniform density.\n' );

  if ( nargin < 1 )
    n = 100;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_TRIANGLE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of N was supplied.\n' );
    fprintf ( 1, '  N is the number of generators.\n' );
    fprintf ( 1, '  A default value N = %d will be used.\n', n );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified number of generators = %d\n',  n );
  end

  if ( nargin < 2 )
    sample_num = 1000 * n;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_TRIANGLE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of SAMPLE_NUM was supplied.\n' );
    fprintf ( 1, '  SAMPLE_NUM is the number of sample points.\n' );
    fprintf ( 1, '  A default value SAMPLE_NUM = %d will be used.\n', ...
      sample_num );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified number of sample points = %d\n', ...
      sample_num );
  end

  if ( nargin < 3 )
    delaunay_display = 0;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_TRIANGLE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of DELAUNAY_DISPLAY was supplied.\n' );
    fprintf ( 1, '  DELAUNAY_DISPLAY is TRUE (nonzero) if the\n' );
    fprintf ( 1, '  Delaunay triangulation is also to be displayed.\n' );
    fprintf ( 1, '  A default value DELAUNAY_DISPLAY = %d will be used.\n', ...
      delaunay_display );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified DELAUNAY_DISPLAY = %d\n', ...
      delaunay_display );
  end
%
%  This switch is set to 1 (TRUE) if the ACCUMARRAY command is available.
%  That speeds up the calculation a lot.  If you don't have the ACCUMARRAY
%  command, just set this to 0.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_TRIANGLE_UNIFORM:\n' );
  fprintf ( 1, '  MATLAB''s ACCUMARRAY command can be used for faster\n' );
  fprintf ( 1, '  computation.  This command is not available in\n' );
  fprintf ( 1, '  some versions of MATLAB.  If ACCUMARRAY is available,\n' );
  fprintf ( 1, '  simply make sure that the ACCUMARAY_AVAILABLE variable\n' );
  fprintf ( 1, '  is set to 1!\n' );

  accumarray_available = 1;

  if ( accumarray_available )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  The ACCUMARRAY command will be used.\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  The ACCUMARRAY command will NOT be used.\n' );
  end
%
%  Clear the figure screen, if already open.
%
  clf
%
%  Randomize the initial locations of the generators.
%
  p(1:2,1:n) = triangle_uniform ( triangle, n );

  it = 0;

  while ( 1 )
%
%  Compute the Delaunay triangle information T for the current nodes.
%
    triangulation = delaunay ( p(1,:), p(2,:) );
%
%  Display the Delaunay triangulation, if requested.
%
    if ( delaunay_display )
      subplot ( 1, 2, 2 )
      trimesh ( triangulation, p(:,1), p(:,2), zeros(n,1) )
      axis ( [ -0.1, 1.1, -0.1, 1.1 ] )
      line ( [ triangle(1,1), triangle(1,2), triangle(1,3), triangle(1,1) ], ...
           [ triangle(2,1), triangle(2,2), triangle(2,3), triangle(2,1) ], ...
        'Color', 'r' );
      title_string = sprintf ( 'Delaunay, step %d', it );
      title ( title_string );
      axis equal
      view ( 2 )
    end
%
%  Display the CVT generators, and the associated Voronoi diagram.
%
    if ( delaunay_display )
      subplot ( 1, 2, 1 )
    end

    voronoi ( p(1,:), p(2,:), triangulation );

    axis ( [ -0.1, 1.1, -0.1, 1.1 ] )
    line ( [ triangle(1,1), triangle(1,2), triangle(1,3), triangle(1,1) ], ...
           [ triangle(2,1), triangle(2,2), triangle(2,3), triangle(2,1) ], ...
      'Color', 'r' );
    title_string = sprintf ( 'Voronoi, step %d', it );
    title ( title_string );
    axis equal
    drawnow
%
%  Generate sample points.
%
%  These sample points implicitly define the geometry of the region.
%  If the region is not a unit square, then the range of the sample
%  data must be changed.
%
%  The data is sampled uniformly.  If a nonuniform density is desired,
%  then the sampling must be done in a biased way.
%
    s = triangle_uniform ( triangle, sample_num );
%
%  For each sample point, find K, the index of the nearest generator.
%  We do this efficiently by using the Delaunay information with
%  Matlab's DSEARCH command, rather than a brute force nearest neighbor
%  computation.
%
    k(1:sample_num,1) = dsearch ( p(1,:), p(2,:), triangulation, s(1,:), s(2,:) );
%
%  The centroid of the Voronoi region associated with each generator
%  is approximated by the average of the sample points it was closest to.
%
    if ( accumarray_available )

      count(1:n) = accumarray ( k, ones(sample_num,1) );
      centroid(1,1:n) = accumarray ( k, s(1,:) );
      centroid(2,1:n) = accumarray ( k, s(2,:) );

    else

      count(1:n) = 0;
      centroid(1,1:n) = 0.0;
      centroid(2,1:n) = 0.0;

      for i = 1 : sample_num
        j = k(i);
        count(j) = count(j) + 1;
        centroid(1,j) = centroid(1,j) + s(1,i);
        centroid(2,j) = centroid(2,j) + s(2,i);
      end

    end
%
%  Replace the generators by the centroids.
%
    p(1,1:n) = ( centroid(1,1:n) ./ count(1:n) )';
    p(2,1:n) = ( centroid(2,1:n) ./ count(1:n) )';

    string = input ( 'RETURN, or Q to quit: ', 's' );

    if ( string == 'q' | string == 'Q' )
        break
    end

    it = it + 1;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_TRIANGLE_UNIFORM:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function p = triangle_uniform ( t, n )

%*****************************************************************************80
%
%% TRIANGLE_UNIFORM returns random points in a triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the vertices of the triangle.
%
%    Input, integer N, the number of points to generate.
%
%    Output, real P(2,N), random points in the triangle.
%
  dim_num = 2;
  alpha = rand ( 1, n );
%
%  Interpret R as a percentage of the triangle's area.
%
%  Imagine a line L, parallel to side 1, so that the area between
%  vertex 1 and line L is R percent of the full triangle's area.
%
%  The line L will intersect sides 2 and 3 at a fraction
%  ALPHA = SQRT ( R ) of the distance from vertex 1 to vertices 2 and 3.
%
  alpha(1:n) = sqrt ( alpha(1:n) );
%
%  Determine the coordinates of the points on sides 2 and 3 intersected
%  by line L.
%
  for dim = 1 : dim_num
    p12(dim,1:n) = ( 1.0 - alpha(1:n) ) * t(dim,1) ...
                         + alpha(1:n)   * t(dim,2);

    p13(dim,1:n) = ( 1.0 - alpha(1:n) ) * t(dim,1) ...
                         + alpha(1:n)   * t(dim,3);
  end
%
%  Now choose, uniformly at random, a point on the line L.
%
  alpha = rand ( 1, n );

  for dim = 1 : dim_num
    p(dim,1:n) = ( 1.0 - alpha(1:n) ) .* p12(dim,1:n) ...
                       + alpha(1:n)   .* p13(dim,1:n);
  end

 return
end


function [p2_closest, ip2_closest, dmin] = nearest_point(p2, p1, ith_closest)
%
% Usage:
%
%    [p2_closest ip2_closest] = nearest_point(p2,p1)
%
% Description:
%
%    Find the points in p2 that are nearest to the points in p1.
%
% AUTHOR: Jay Dubb (jdubb@nmr.mgh.harvard.edu)
% DATE:   11/13/2008
%
% Updates:
%
% 01/21/2010 - Jay Dubb changed variable names and modified comment
%              to better describe the way this function works.
%

% Output arguments
p2_closest  = [];
ip2_closest = [];
dmin        = 0;

if isempty(p1) || isempty(p2)
    return;
end
if ~exist('ith_closest','var') | isempty(ith_closest)
    ith_closest = 1;
end

% Figure out the dimentions of p1 and p2 to see if they're compatible
sz1 = size(p1);
sz2 = size(p2);
ndim = max(sz1(sz1==sz2));
if isempty(ndim)
    if length(sz1)>2 || length(sz1)~=length(sz2)
        MenuBox('nearest_point error: p1 and p2 number of dimensions incompatible','OK');
        return;
    end
    if length(sz1)>2 || length(sz1)~=length(sz2)
        MenuBox('nearest_point error: p1 and p2 number of dimensions incompatible','OK');
        return;
    end
    if ismember(1,sz1) && ismember(1,sz2)
        ndim=1;
    end
end
if ndim~=3 && ndim~=1
    MenuBox('nearest_point warning: currently only handles arguments in 1 and 3 dimensions.','OK');
    return;
end

m=size(p1,1);
n=size(p2,1);

if ith_closest>n
    fprintf('Error: %dth closest element is greater than size of first argument (%d)\n', ith_closest, n);
    return;
end

if ndim==3
    p2_closest  = zeros(m,3);
    ip2_closest = zeros(m,1);
    dmin = zeros(m,1);
    for k=1:m
        d = sqrt((p2(:,1)-p1(k,1)).^2+(p2(:,2)-p1(k,2)).^2+(p2(:,3)-p1(k,3)).^2);
        [d2, j] = sort(d);
        dmin(k) = d2(ith_closest);

        ip2_closest(k)  = j(ith_closest);
        p2_closest(k,:) = p2(ip2_closest(k),:);
    end
elseif ndim==1
    p2_closest  = zeros(m,1);
    ip2_closest = zeros(m,1);
    dmin = zeros(m,1);
    for k=1:m
        d = sqrt((p2(:)-p1(k)).^2);
        [d2, j] = sort(d);
        dmin(k) = d2(ith_closest);

        ip2_closest(k) = j(ith_closest);
        p2_closest(k)  = p2(ip2_closest(k));
    end
end


function ihs_test01 ( )

%*****************************************************************************80
%
%% IHS_TEST01 tests the improved distributed hypercube sampling algorithm.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 March 2004
%
%  Author:
%
%    John Burkardt
%
  point_num = 10;
  duplication = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'IHS_TEST01' );
  fprintf ( 1, '  IHS implements the IHS Algorithm\n' );
  fprintf ( 1, '  (Improved Distributed Hypercube Sampling)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Demonstrate the code for a fixed number of points\n' );
  fprintf ( 1, '  and an increasing dimension.\n' );

  for dim_num = 1 : 4

    opt = point_num / point_num^( 1.0 / dim_num );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Spatial dimension =        %d\n', dim_num );
    fprintf ( 1, '  Number of points =         %d\n', point_num );
    fprintf ( 1, '  Duplication factor =       %d\n', duplication );
    fprintf ( 1, '  Desired minimum distance = %f\n', opt );
%
%  Get the points.
%
    x = ihs ( dim_num, point_num, duplication );
%
%  Compute the covariance.
%
    [ average, sd, covc ] = covariance ( dim_num, point_num, x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Average minimum distance %f\n', average );
    fprintf ( 1, '  Standard deviation:      %f\n', sd );
    fprintf ( 1, '  Covariance:              %f\n', covc );

    fprintf ( 1, '\n' );

    for j = 1 : point_num
      fprintf ( 1, '%4d    ', j );
      for i = 1 : dim_num
        fprintf ( 1, '%4d  ', x(i,j) );
      end
      fprintf ( 1, '\n');
    end

  end

  return
end


function [ r, seed ] = r8vec_uniform_01 ( n, seed )

%*****************************************************************************80
%
%% R8VEC_UNIFORM_01 returns a unit pseudorandom R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Second Edition,
%    Springer, 1987,
%    ISBN: 0387964673,
%    LC: QA76.9.C65.B73.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, December 1986, pages 362-376.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998,
%    ISBN: 0471134031,
%    LC: T57.62.H37.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, Number 2, 1969, pages 136-143.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real R(N), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  i4_huge = 2147483647;

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8VEC_UNIFORM_01 - Fatal error!' );
  end

  for i = 1 : n

    k = floor ( seed / 127773 );

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

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

    r(i) = seed * 4.656612875E-10;

  end

  return
end


function pass = test_matrixOutput(~)
% TEST_MATRIXOUTPUT   Chebmatrix operations resulting only in doubles should
% return a normal matrix.

dom = [-3 1];
% A chebmatrix with 5 column functions
T = chebpoly(1:5, dom);
T = chebmatrix(T);
C = T'*T;
pass(1) = isnumeric(C) && all( size(C) == [5 5]);

% Two rows, 5 columns
TT = [T; T];
CC = TT'*TT;
pass(2) = isnumeric(CC) && all( size(CC) == [5 5]);

% Operators involved, output should not be numeric
[Z, I, D, C, M] = linop.primitiveOperators(dom);
A = [I D; Z C];
f = [chebpoly(1, dom); chebpoly(5, dom)];
Af = A*f;
pass(3) = isa(Af, 'chebmatrix') && ...
    all(cellfun(@isa, Af.blocks, {'chebfun'; 'chebfun'})) && ...
    all( size(Af) == [2 1]);

% Functionals involved, output should be numeric
[Z, E, S, D] = linop.primitiveFunctionals(dom);
J = [Z-E(1), S];
Jf = J*f;
pass(4) = isnumeric(Jf) && all(size(Jf) == [1 1]);
K = [J' J'];
Kf = K*f;
pass(5) = isnumeric(Kf) && all(size(Kf) == [2 1]);
end


clear all
clc

% s - number of sparse coefficients
% N - length of each input vector
% n - length of output vector
% r - number of measurements

s = 15; N = 100; n = 60; r = 10;

t = randperm(N);
X0 = zeros(N,r);

for i = 1:r
    X0(t(1:s),i) = randn(s,1);
end

M = randn(n,N); Mop = opMatrix(M);
Sop = opDCT(N);

S = max(svd(M));

%%
F = opFoG(Mop, Sop);

for i = 1:r
    Y(:,i) = F(X0(:,i),1);
end

X = UnconSynthMMV(Y, F, 1e-6, 10, S);
norm(X0-X,'fro')/norm(X0,'fro')

X = SynthMMV(Y, F, 1e-6, S);
norm(X0-X,'fro')/norm(X0,'fro')

%%
for i = 1:r
    Z0(:,i) = Sop(X0(:,i),2);
    Y(:,i) = Mop(Z0(:,i),1);
end

Z = UnconAnaMMV(Y, M, Sop, 1e-6, 10, S, 1);
norm(Z0-Z,'fro')/norm(Z0,'fro')

Z = AnaMMV(Y, M, Sop, 1e-6, S, 1);
norm(Z0-Z,'fro')/norm(Z0,'fro')


function e = rmModelSearchFit_oneGaussianNonlinear(p,Y,Xv,Yv,stim, hrf, scan_num, t)
% rmModelSearchFit_oneGaussianNonlinear - actual fit function of rmSearchFit
%
% error = rmModelSearchFit(p,Y,trends,Xgrid,YGrid,stimulusMatrix);
%
% Basic barebones fit of a single time-series. Error is returned in
% percentage: 100% is RSS of unfitted time-series. This way we can quantify
% the improvement of the fit independend of the variation in the raw
% time-series.
%
% 2006/06 SOD: wrote it.
% 2006/12 SOD: modifications for fmincon, this is litterally called >10000
% times so we cut every corner possible.
% 2010/02 SOD: evaluated lscov this did not improve performance here (using
% profiler)

% make RF (taken from rfGaussian2d)
Xv = Xv - p(1);   % positive x0 moves center right
Yv = Yv - p(2);   % positive y0 moves center up
RF = exp( (Yv.*Yv + Xv.*Xv) ./ (-2.*(p(3).^2)) );

% make prediction (taken from rfMakePrediction)
pred = (stim*RF).^p(4);

for scan = 1:numel(hrf)
    inds = scan_num == scan;
    pred(inds,:) = filter(hrf{scan}, 1, pred(inds,:));
end

X = [pred t];

% fit - inlining pinv
%b = pinv(X)*Y;
[U,S,V] = svd(X,0);

s = diag(S);
tol = numel(X) * eps(max(s));
r = sum(s > tol);
if (r == 0)
    pinvX = zeros(size(X'));
else
    s = diag(ones(r,1)./s(1:r));
    pinvX = V(:,1:r)*s*U(:,1:r)';
end
b = pinvX*Y;

% compute residual sum of squares (e)
% e = norm(Y - X*abs(b));
if b(1)>0,
    e = norm(Y - X*b);
else
    e = norm(Y).*(1+sum(abs(b(1))));
end
return;