function [r,bb,f, han] = timeseries_prplot(y,X,cols,varargin)
% :Usage:
% ::
%
% Partial residual plot of a subset [cols] of columns regressing y on X
%
%    [r,bb,f] = timeseries_prplot(y,X,cols,varargin)
%
% - Plots timeseries data (y') removing other regressors (~cols) against partial fitted response (X[cols])
% - Makes sure there is an intercept at the end, whether one was entered or not.
% - This is *not* a partial regression plot
%
% See also: https://www.itl.nist.gov/div898/software/dataplot/refman1/auxillar/partregr.htm#:~:text=Partial%20regression%20plots%20are%20most,independent%20variables%20in%20the%20model).
% "Partial regression plots are most commonly used to identify leverage
% points and influential data points that might not be leverage points.
% Partial residual plots are most commonly used to identify the nature of the
% relationship between Y and Xi (given the effect of the other independent variables
% in the model)"
%
% :Inputs:
%
%   **y:**
%        y is n x 1 data points, X is n x k model matrix of predictors
%
%        y is adjusted to remove all effects OTHER THAN those columns of X
%        specified in cols, creating a partial residual vector y'.
%
%   **X:**
%        The fitted response to X(:,cols) is plotted against y' to graphically assess the
%        effect of particular columns of X on y.
%
%   varargin includes two optional arguments (enter both *together*):
%   These will create "boxcars" showing trials onsets, e.g., for fMRI
%     1) a vector of trial onsets (to shade in plot)
%     2) length of elements to shade after trial onset.
%
% :Outputs:
%
%   **r:**
%        partial residuals
%
%   **bb:**
%        betas
%
%   **f:**
%        partial fitted response
%
% :Examples:
% ::
%
%    timeseries_prplot(Yfla,X,[2 4],x2,18);
%
% to average across sessions 1 and 2:
%
% ..
%    Tor Wager
% ..

% Make sure there is an intercept at the end, whether one was entered or not.
X = intercept(X, 'end');

% get betas from full model
b = pinv(X) * y;
bb = b;

% get fit from subset of predictors of interest here

f = X(:,cols) * b(cols);

% get partial residuals, excluding the subset of interest

X(:,cols) = []; b(cols) = [];
r = y - X * b;

% plot
tor_fig; hold on;
han = plot(r,'k','LineWidth', 2);
han(2) = plot(f,'b','LineWidth', 2);
legend({'Partial residual (data)' 'Partial fit'});

if length(varargin) > 0
    x2 = varargin{1}; len = varargin{2};
    sc = 2 * max(abs(r));

    for i = 1:length(x2), hh(i) = fill([x2(i) x2(i)+len x2(i)+len, x2(i)],sc*([0 0 1 1]-.5),[.5 .5 .5]); end

    set(hh,'EdgeColor','none','FaceAlpha',.5)
end

return


function R = functionRlocalscattering(M,theta,ASDdeg,antennaSpacing,distribution)
%Generate the spatial correlation matrix for the local scattering model,
%defined in (2.23) for different angular distributions.
%
%INPUT:
%M              = Number of antennas
%theta          = Nominal angle
%ASDdeg         = Angular standard deviation around the nominal angle
%                 (measured in degrees)
%antennaSpacing = (Optional) Spacing between antennas (in wavelengths)
%distribution   = (Optional) Choose between 'Gaussian', 'Uniform', and
%                'Laplace' angular distribution. Gaussian is default
%
%OUTPUT:
%R              = M x M spatial correlation matrix
%
%
%This Matlab function was developed to generate simulation results to:
%
%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.1 (Last edited: 2017-11-16)
%
%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.

%Set the antenna spacing if not specified by input
if  nargin < 4

    %Half a wavelength distance
    antennaSpacing = 1/2;

end

%Set angular distribution to Gaussian if not specified by input
if nargin<5
    distribution = 'Gaussian';
end

%Compute the ASD in radians based on input
ASD = ASDdeg*pi/180;

%The correlation matrix has a Toeplitz structure, so we only need to
%compute the first row of the matrix
firstRow = zeros(M,1);

%Go through all the columns of the first row
for column = 1:M

    %Distance from the first antenna
    distance = antennaSpacing*(column-1);

    %For Gaussian angular distribution
    if strcmp(distribution,'Gaussian')

        %Define integrand of (2.23)
        F = @(Delta)exp(1i*2*pi*distance*sin(theta+Delta)).*exp(-Delta.^2/(2*ASD^2))/(sqrt(2*pi)*ASD);

        %Compute the integral in (2.23) by including 20 standard deviations
        firstRow(column) = integral(F,-20*ASD,20*ASD);

    %For uniform angular distribution
    elseif strcmp(distribution,'Uniform')

        %Set the upper and lower limit of the uniform distribution
        limits = sqrt(3)*ASD;

        %Define integrand of (2.23)
        F = @(Delta)exp(1i*2*pi*distance*sin(theta+Delta))/(2*limits);

        %Compute the integral in (2.23) over the entire interval
        firstRow(column) = integral(F,-limits,limits);

    %For Laplace angular distribution
    elseif strcmp(distribution,'Laplace')

        %Set the scale parameter of the Laplace distribution
        LaplaceScale = ASD/sqrt(2);

        %Define integrand of (2.23)
        F = @(Delta)exp(1i*2*pi*distance*sin(theta+Delta)).*exp(-abs(Delta)/LaplaceScale)/(2*LaplaceScale);

        %Compute the integral in (2.23) by including 20 standard deviations
        firstRow(column) = integral(F,-20*ASD,20*ASD);

    end

end

%Compute the spatial correlation matrix by utilizing the Toeplitz structure
R = toeplitz(firstRow);


function [ fea, out ] = ex_convdiff4( varargin )
%EX_CONVDIFF4 1D Burgers equation (convection and diffusion) example.
%
%   [ FEA, OUT ] = EX_CONVDIFF4( VARARGIN ) 1D Burgers equation with steady solution,
%   u_t + (b*u-c)*u_x - nu*u_xx = 0 with exact solution c/b*(1-tanh(c/(2*nu)*(x-x0))).
%   Tests both time dependent and steady solvers. Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       b           scalar {1.0}           Strength of nonlinearity
%       c           scalar {0.13}          Convection velocity
%       nu          scalar {0.01}          Diffusion coefficient
%       x0          scalar {0.5}           Posision of smooth shock
%       hmax        scalar {1/25}          Max grid cell size
%       ischeme     scalar {-1}            Solver scheme (<0 = stationary)
%       nsolve      scalar {2}             Nonlinear solver (when ischeme<0)
%       dt          scalar {0.1}           Time step size
%       sfun        string {sflag1}        Shape function
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
            'b',        1.0; ...
            'c',        0.13; ...
            'nu',       0.01; ...
            'x0',       0.5; ...
            'hmax',     1/20; ...
            'ischeme'   -1; ...
            'nsolve'    2; ...
            'dt'        0.1; ...
            'sfun',     'sflag1'; ...
            'iplot',    1; ...
            'tol',      1e-2; ...
            'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

b  = opt.b;
c  = opt.c;
nu = opt.nu;
x0 = opt.x0;
refsol = [num2str(c/b),'*(1-tanh(',num2str(c/(2*nu)),'*(x-',num2str(x0),')))'];
bcd1   = c/b*(1-tanh((c/(2*nu)*(0-x0))));
bcd2   = c/b*(1-tanh((c/(2*nu)*(1-x0))));

% Grid generation.
fea.grid = linegrid( 1/opt.hmax, 0, 1 );

% Problem definition.
fea.sdim  = { 'x' };
fea = addphys( fea, @convectiondiffusion );
fea.phys.cd.sfun = { opt.sfun };
fea.phys.cd.eqn.coef{2,4} = { opt.nu };
fea.phys.cd.eqn.coef{3,4} = { [num2str(b),'*c-',num2str(c)] };
fea = parsephys(fea);

% Parse and solve problem.
fea = parseprob( fea );
fea.bdr.d{1} = bcd1;
fea.bdr.d{2} = bcd2;
fea.bdr.n    = cell(1,2);
x = fea.grid.p';
if( strcmp( opt.sfun,'sflag2' ) )
  x = [ x; (x(2:end)+x(1:end-1))/2 ];
end
if( opt.ischeme<0 )
  init = 0;
  jac.form  = {[1;1]};
  jac.coef  = {[num2str(b),'*cx']};
  fea.sol.u = solvestat( fea, 'fid', fid, 'init', init, 'maxnit', 1000, 'nsolve', 2, 'jac', jac, 'nsolve', opt.nsolve );
else
  init = [num2str(bcd1),'+x*',num2str(bcd2-bcd1)];
  [fea.sol.u,tlist] = solvetime( fea, 'fid', fid, 'init', init, 'ischeme', opt.ischeme, 'tstep', opt.dt, 'tmax', 10 );
end

% Postprocessing.
if( opt.iplot>0 )
  figure
  i_sol = size(fea.sol.u,2);
  [~,ix] = sort( x );
  postplot( fea, 'surfexpr', 'c', 'solnum', i_sol );
  hold on
  u_r = real( eval( refsol ) );
  plot( sort(x), u_r(ix), 'r--' );
  title( 'Steady solution' )
  xlabel( 'x' )
  drawnow
end

% Error checking.
i_sol = size(fea.sol.u,2);
u_i   = fea.sol.u(:,i_sol);
u_r   = real( eval( refsol ) );
errnm = norm( u_i - u_r )/norm( u_r );
out.err  = errnm;
out.pass = all( errnm<opt.tol );

if ( nargout==0 )
  clear fea out
end


clear all, close all, clc
%% Create a simple signal with two frequencies
dt = .001;
t = 0:dt:1;
f = sin(2*pi*50*t) + sin(2*pi*120*t); % Sum of 2 frequencies
f = f + 2.5*randn(size(t));  %  Add some noise

%% Compute the Fast Fourier Transform FFT
n = length(t);
fhat = fft(f,n);       % Compute the fast Fourier transform
PSD = fhat.*conj(fhat)/n; % Power spectrum (power per freq)
freq = 1/(dt*n)*(0:n); % Create x-axis of frequencies in Hz
L = 1:floor(n/2);   % Only plot the first half of freqs

%% Use the PSD to filter out noise
indices = PSD>100;  % Find all freqs with large power
PSDclean = PSD.*indices;  % Zero out all others
fhat = indices.*fhat;  % Zero out small Fourier coeffs. in Y
ffilt = ifft(fhat); % Inverse FFT for filtered time signal

%% PLOTS
subplot(3,1,1)
plot(t,f,'r','LineWidth',1.2), hold on
plot(t,f,'k','LineWidth',1.5)
legend('Noisy','Clean')

subplot(3,1,2)
plot(t,f,'k','LineWidth',1.5), hold on
plot(t,ffilt,'b','LineWidth',1.2)
legend('Clean','Filtered')

subplot(3,1,3)
plot(freq(L),PSD(L),'r','LineWidth',1.5), hold on
plot(freq(L),PSDclean(L),'-b','LineWidth',1.2)
legend('Noisy','Filtered')

%
% %% PLOTS
% subplot(3,1,1)
% plot(t,y,'r','LineWidth',1.2)
% hold on
% plot(t,x,'k','LineWidth',1.5)
% axis([0 .25 -5 5])
% legend('Noisy','Clean')
%
% subplot(3,1,2)
% plot(t,x,'k','LineWidth',1.5)
% hold on
% plot(t,yfilt,'b','LineWidth',1.2)
% axis([0 .25 -5 5])
% legend('Clean','Filtered')
%
% subplot(3,1,3)
% plot(freq(L),PSD(L),'r','LineWidth',1.5)
% hold on
% plot(freq(L),PSDclean(L),'-b','LineWidth',1.2)
% legend('Noisy','Filtered')


function [ mB ] = ApplyGaussianBlur( mI, gaussianKernelStd, stdToRadiusFactor )

gaussianBlurRadius  = ceil(stdToRadiusFactor * gaussianKernelStd);

vGaussianKernel = exp(-((-gaussianBlurRadius:gaussianBlurRadius) .^ 2) / (2 * gaussianKernelStd * gaussianKernelStd));
vGaussianKernel = vGaussianKernel / sum(vGaussianKernel);

mI   = PadArrayReplicate(mI, gaussianBlurRadius);

mB = conv2(vGaussianKernel, vGaussianKernel.', mI, 'valid');

end


clear; close all; clc;
%% easiest example of curl: a stick on a stream

[x,y]=meshgrid(0:1/10:1.5);
u=y;
v=zeros(size(x));

figure;
quiver(x,y,u,v);
% grid on;
axis([0 1 0 1])

%% easy example for curl: Vortex
figure('position',[542, 137, 750, 627]);
U_i = 10;% free stream velocity
C = 1000/(2*pi);% vortex strength
meshfactor = 10;
[x,y] = meshgrid(-U_i:U_i/meshfactor:U_i);

x1 = 0;
y1 = 0;
u1 = U_i - C*(y-y1)./(2*((x-x1).^2 + (y-y1).^2)); %%define the velocity component in the x
v1 = C*(x-x1)./(2*((x-x1).^2 + (y-y1).^2)); %%define the velocity component in the y

x2 = 4;
y2 = 5;
u2 = U_i + C*(y-y2)./(2*((x-x2).^2 + (y-y2).^2)); %%define the velocity component in the x
v2 = -C*(x-x2)./(2*((x-x2).^2 + (y-y2).^2)); %%define the velocity component in the y

u = u1+u2;
v = v1+v2;

[r,c]=find(isnan(u));

for i = 1:2
    u(r(i), c(i))=mean([u(r(i)-1,c(i)),u(r(i)+1,c(i))]);
    v(r(i), c(i))=mean([v(r(i)-1,c(i)),v(r(i)+1,c(i))]);
end

quiver(x,y,u,v)
xlim([-U_i, U_i])
ylim([-U_i, U_i])
set(gcf,'position',[542 137 750 627])

%
set(gcf,'color','w');
hold on;
[verts,averts] = streamslice(x,y,u,v);
sl = streamline([verts averts]);

iverts = interpstreamspeed(x,y,u,v,verts,0.01);
streamparticles(iverts,100,'Animate',15,'FrameRate',40,'Markersize',5)

%% Vortex represented with curl

figure;
ccurl=curl(x,y,u,v);
pcolor(x,y,ccurl);
hold on
shading interp
colormap(jet);

quiver(x,y,u,v);
xlim([-U_i, U_i])
ylim([-U_i, U_i])
set(gcf,'position',[542 137 750 627])

%% ex: wind
k = 4;
load wind
x = x(:,:,k);
y = y(:,:,k);
u = u(:,:,k);
v = v(:,:,k);
figure;
cav = curl(x,y,u,v);
pcolor(x,y,cav);
shading interp
hold on
quiver(x,y,u,v,'y','color','k');
hold off
colormap('jet');


%Author: Moeti Ncube

%Comments: This code calibrates the heston model to any dataset of the form
%of the marketdata.txt file. It also simulates the heston model given the
%optimized parameters.

%Marketdata=[strike, maturity,impvol,time till expiry]

%Strike: Strike price of options
%Maturity: (1=Prompt month, 2=Prompt month+1,...)
%Impvol: Market Implied vol
%Time till expiry: Days until option expires

clear all

global impvol; global strike; global T; global F0; global r;

%Initial Parameter Guess for Hestion Model
%V(1), Kappa, Theta, Vol of Volatility (sig), Correlation (rho)
x0=[.5,.5,.5,.05,.5];
%Constraints (Lower and Upper bounds on Parameters)
lb = [0, 0, 0, 0, -.9];
ub = [1, 100, 1, .5, .9];
%Number of MCMC simulations of Heston Model
M=50000;
%Current Asset Price (Prompt Month Price) and Interest rate
F0=1250; r=0;

load marketdata.txt

T=marketdata(:,4)/365;
strike=marketdata(:,1);
impvol=marketdata(:,3);

%Optimization
x = lsqnonlin(@costf2,x0,lb,ub);

for k=1:length(T);
%Initial asset price
shes(1)=F0;
%Number of Time Steps,time step size
N=round(T(k)/(1/360));dt=T(k)/N;

%Heston Parameters
vhes(1)=x(1);  kappa=x(2); theta=x(3); vsigma=x(4);rho=x(5); simPath=0;

%Simulation of Heston Model
for i = 1:M
for j=1:N
%heston model
r1 = randn;
r2 = rho*r1+sqrt(1-rho^2)*randn;
shes(j+1)=shes(j)*exp((-0.5*vhes(j))*dt+sqrt(vhes(j))*sqrt(dt)*r1);
vhes(j+1)=vhes(j)*exp(((kappa*(theta - vhes(j))-0.5*vsigma^2)*dt)/vhes(j) + vsigma*(1/sqrt(vhes(j)))*sqrt(dt)*r2);
end
simPath = simPath + exp(-r*T(k)) * max(shes(j+1) - strike(k), 0);
end
simhes(k)=simPath/M;
simimpvol(k)=blkimpv(shes(1), strike(k), r, T(k), simhes(k));
modhes(k)=HestonCall(shes(1),strike(k),r,T(k),vhes(1),kappa,theta,vsigma,rho,0);
hesimpvol(k)=blkimpv(shes(1), strike(k), r, T(k), modhes(k));
end

for i=1:length(T)
bsprice(i)=blsprice(F0,strike(i),r,T(i),impvol(i));
end

%Output optimized Parameters
x

%Compare blackscholes option price, analytical heston price, and simulated
%heston prices for a given maturity and strike
pricedata=[T,strike,bsprice',modhes',simhes']

%Compare blackscholes option IV, analytical heston IV, and simulated
%heston IV for a given maturity and strike

voldata=[T,strike,impvol,hesimpvol',simimpvol']


function p = hex_grid_points ( nodes_per_layer, layers, n, box )

%*****************************************************************************80
%
%% HEX_GRID_POINTS returns coordinate box hex grid points.
%
%  Discussion:
%
%    This routine determines the coordinates of the elements of
%    a hexagonal grid in the unit square.
%
%    A hexagonal grid is defined in the coordinate box [A,B] x [C,D].
%
%    All nodes of the grid lie on one of LAYERS horizontal lines.
%    The first of these lines is from (A,C) to (B,C), and each
%    successive line is HY units higher.
%
%    On all the odd numbered lines, there are NODES_PER_LAYER points,
%    equally spaced from A to B, with a spacing of HX.
%
%    On the even numbered lines, there are NODES_PER_LAYER-1 points,
%    whose values are the midpoints of successive intervals on
%    an odd numbered line.  (The grid is staggered).
%
%    HY = HX * sqrt ( 3 ) / 2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 March 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODES_PER_LAYER, the number of grid points on the first
%    horizontal layer of points.
%
%    Input, integer LAYERS, the number of horizontal layers.
%
%    Input, integer N, the total number of hex grid points.
%
%    Input, real BOX(2,2), the values of A, B, C and D
%    that define the coordinate box.
%
%    Output, real P(2,N), the coordinates of the
%    mesh points, listed one horizontal layer at a time.
%
  ndim = 2;

  if ( nodes_per_layer < 1 )
    p = [];
    return
  end

  if ( nodes_per_layer == 1 )
    p(1:ndim,1) = ( box(1:ndim,1) + box(1:ndim,2) ) / 2.0;
    return
  end

  [ hx, hy ] = hex_grid_h ( nodes_per_layer, box );

  k = 0;

  for j = 1 : layers

    y = box(2,1) + hy * ( j - 1 );

    jmod = mod ( j, 2 );

    if ( jmod == 1 )

      for i = 1 : nodes_per_layer
        x = box(1,1) + ( box(1,2) - box(1,1) ) * ( i - 1 ) ...
          / ( nodes_per_layer - 1 );
        k = k + 1;
        if ( k <= n )
          p(1,k) = x;
          p(2,k) = y;
        end
      end

    else

      for i = 1 : nodes_per_layer-1
        x = box(1,1) + ( box(1,2) - box(1,1) ) * ( 2 * i - 1 ) ...
          / ( 2 * nodes_per_layer - 2 );
        k = k + 1;
        if ( k <= n )
          p(1,k) = x;
          p(2,k) = y;
        end
      end

    end

  end

  return
end


function value = r8_beta_pdf ( alpha, beta, rval )

%*****************************************************************************80
%
%% R8_BETA_PDF evaluates the PDF of a beta distribution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2013
%
%  Author:
%
%    Original FORTRAN90 version by Guannan Zhang.
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, real ALPHA, BETA, shape parameters.
%    0.0 < ALPHA, BETA.
%
%    Input, real RVAL, the point where the PDF is evaluated.
%
%    Output, real VALUE, the value of the PDF at RVAL.
%
  if ( alpha <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BETA_PDF - Fatal error!\n' );
    fprintf ( 1, '  Parameter ALPHA is not positive.\n' );
    error ( 'R8_BETA_PDF - Fatal error!' );
  end

  if ( beta <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BETA_PDF - Fatal error!\n' );
    fprintf ( 1, '  Parameter BETA is not positive.\n' );
    error ( 'R8_BETA_PDF - Fatal error!' );
  end

  if ( rval < 0.0 || 1.0 < rval )

    value = 0.0;

  else

    temp = r8_gamma_log ( alpha + beta ) - r8_gamma_log ( alpha ) ...
      - r8_gamma_log ( beta );

    value = exp ( temp ) * rval ^ ( alpha - 1.0 ) ...
      * ( 1.0 - rval ) ^ ( beta - 1.0 );

  end

  return
end


function a = jacobi ( m, n )

%*****************************************************************************80
%
%% JACOBI returns the JACOBI matrix.
%
%  Formula:
%
%    if ( J = I - 1 )
%      A(I,J) = 0.5 * sqrt ( ( 4 * J^2 ) / ( 4 * J^2 - 1 ) )
%    else if ( J = I + 1 )
%      A(I,J) = 0.5 * sqrt ( ( 4 * (J-1)^2 ) / ( 4 * (J-1)^2 - 1 ) )
%    else
%      A(I,J) = 0
%
%  Example:
%
%    M = 4, N = 4
%
%    0            0.577350269  0            0
%    0.577350269  0            0.516397779  0
%    0            0.516397779  0            0.507092553
%    0            0            0.507092553  0
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A has a zero diagonal.
%
%    The eigenvalues of A are the zeros of the Legendre polynomial
%    of degree N.  They lie symmetrically in [-1,1], and are also
%    the nodes of Gauss-Legendre quadrature.  For the case of N = 4,
%    these eigenvalues are
%
%      [ -0.861136312, -0.339981044, +0.339981044, +0.861136312 ].
%
%    It follows that A is singular when N is odd.
%
%    The J-th Gauss-Legendre weight is twice the square of the first
%    component of the J-th eigenvector of A.  For the case of N = 4,
%    the eigenvector matrix is:
%
%      -0.417046     -0.571028     -0.571028    0.417046
%       0.622037      0.336258     -0.336258    0.622038
%      -0.571028      0.417046      0.417046    0.571028
%       0.336258     -0.622037      0.622038    0.336258
%
%    and the corresponding weights are
%
%      [ 0.347854845, 0.652145155, 0.652145155, 0.347854845 ]
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Lloyd Trefethen, David Bau,
%    Numerical Linear Algebra,
%    SIAM, 1997, pages 287-292.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of A.
%
%    Output, real A(M,N), the matrix.
%
  a = zeros ( m, n );

  for i = 1 : m
    for j = 1 : n

      if ( j == i - 1 )
        a(i,j) = 0.5 * sqrt ( ( 4 * j * j ) / ( 4 * j * j - 1 ) );
      elseif ( j == i + 1 )
        a(i,j) = 0.5 * sqrt ( ( 4 * ( j - 1 ) * ( j - 1 ) ) ...
          / ( 4 * ( j - 1 ) * ( j - 1 ) - 1 ) );
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


function [I J] = itriu(sz, k)
% function [I J] = itriu(sz) % OR
% I = itriu(sz) OR
%
% Return the subindices [I J] (or linear indices I if single output call)
% in the purpose of extracting an upper triangular part of the matrix of
% the size SZ. Input k is optional shifting. For k=0, extract from the main
% diagonal. For k>0 -> above the diagonal, k<0 -> below the diagonal
%
% This returnd same as [...] = find(triu(ones(sz),k))
% - Output is a column and sorted with respect to linear indice
% - No intermediate matrix is generated, that could be useful for large
%   size problem
% - Mathematically, A(itriu(size(A)) is called (upper) "half-vectorization"
%   of A
%
% Example:
%
% A = [ 7     5     4
%       4     2     3
%       9     1     9
%       3     5     7 ]
%
% I = itriu(size(A))  % gives [1 5 6 9 10 11]'
% A(I)                % gives [7 5 2 4  3  9]' OR A(triu(A)>0)
%
% Author: Bruno Luong <brunoluong@yahoo.com>
% Date: 21/March/2009

if isscalar(sz)
    sz = [sz sz];
end
m=sz(1);
n=sz(2);

% Main diagonal by default
if nargin<2
    k=0;
end

nc = n-max(k,0); % number of columns of the triangular part
lo = ones(nc,1); % lower row indice for each column
hi = min((1:nc).'-min(k,0),m); % upper row indice for each column

if isempty(lo)
    I = zeros(0,1);
    J = zeros(0,1);
else
    c=cumsum([0; hi-lo]+1); % cumsum of the length
    I = accumarray(c(1:end-1), (lo-[0; hi(1:end-1)]-1), ...
                   [c(end)-1 1]);
    I = cumsum(I+1); % row indice
    J = accumarray(c,1);
    J(1) = 1 + max(k,0); % The row indices starts from this value
    J = cumsum(J(1:end-1)); % column indice
end

if nargout<2
    % convert to linear indices
    I = sub2ind([m n], I, J);
end

end % itriu


function Y = DGradient(X, Dim, Spacing, Method)  %#ok<STOUT,INUSD>
% Gradient along a dimension
% Y = DGradient(X, Dim, Spacing, Method)
% INPUT:
%   X:   Real DOUBLE array.
%   Spacing: Scalar or vector of the length SIZE(X, Dim).
%        A scalar value is the distance between all points, while a vector
%        contains all time points, such that DIFF(Spacing) are the distances.
%        For equally spaced input a scalar Spacing is much faster.
%        Optional, default: 1.0
%   Dim: Dimension to operate on.
%        Optional, default: [] (1st non-singelton dimension).
%   Method: String, order of the applied method for unevenly spaced X:
%        '1stOrder', faster centered differences as in Matlab's GRADIENT.
%        '2ndOrder', 2nd order accurate centered differences.
%        On the edges forward and backward difference are used.
%        Optional, default: '1stOrder'.
%
% OUTPUT:
%   Y:   Gradient of X, same size as X.
%
% EXAMPLES:
%   t = cumsum(rand(1, 100)) + 0.01;  t = 2*pi * t ./ max(t);
%   x = sin(t);
%   dx1 = DGradient(x, t, 2, '1stOrder');
%   dx2 = DGradient(x, t, 2, '2ndOrder');
%   dx  = cos(t);          % Analytic solution
%   h = plot(t, dx, t, dx1, 'or', t, dx2, 'og');  axis('tight');
%   title('cos(x) and DGradient(sin(x))');
%   legend(h, {'analytic', '1st order', '2nd order'}, 'location', 'best');
%
% NOTES:
% - There are a lot of other derivation tools in the FEX. This function is
%   faster, e.g. 25% faster than dqdt and 10 to 16 times faster than Matlab's
%   GRADIENT. In addition it works with multi-dim arrays, on a specific
%   dimension only, and can use a 2nd order method for unevenly spaced data.
% - This function does not use temporary memory for evenly spaced data and if
%   a single vector is processed. Otherwise the 1st-order method needs one and
%   the 2nd-order method 3 temporary vectors of the length of the processed
%   dimension.
% - Matlab's GRADIENT processes all dimensions ever, while DGradient operates on
%   the specified dimension only.
% - 1st order centered difference:
%     y(i) = (x(i+1) - x(i-1) / (s(i+1) - s(i-1))
% - 2nd order centered difference:
%     y(i) = ((x(i+1) * (s(i)-s(i-1)) / (s(i+1)-s(i))) -
%             (x(i-1) * (s(i+1)-s(i)) / (s(i)-s(i-1)))) / (s(i+1)-s(i-1))
%            + x(i) * (1.0 / (s(i)-s(i-1)) - 1.0 / (s(i+1)-s(i)))
%   For evenly spaced X, both methods reply equal values.
%
% COMPILE:
%   mex -O DGradient.c
% Consider C99 comments on Linux:
%   mex -O CFLAGS="\$CFLAGS -std=c99" DGradient.c
% Pre-compiled Mex: http://www.n-simon.de/mex
% Run the unit test uTest_DGradient after compiling.
%
% Tested: Matlab 6.5, 7.7, 7.8, WinXP, 32bit
%         Compiler: LCC2.4/3.8, BCC5.5, OWC1.8, MSVC2008
% Assumed Compatibility: higher Matlab versions, Mac, Linux, 64bit
% Author: Jan Simon, Heidelberg, (C) 2011 matlab.THISYEAR(a)nMINUSsimon.de
%
% See also GRADIENT, DIFF.
% FEX: central_diff (#12 Robert A. Canfield)
%      derivative (#28920, Scott McKinney)
%      movingslope (#16997, John D'Errico)
%      diffxy (#29312, Darren Rowland)
%      dqdt (#11965, Geoff Wawrzyniak)

% $JRev: R0c V:003 Sum:RkNcmbCGEJGT Date:02-Jan-2010 02:41:46 $
% $License: BSD $
% $File: Tools\Mex\Source\DGradient.c $
% History:
% 001: 30-Dec-2010 22:42, First version published under BSD license.

% This is a dummy file to support Matlab's HELP.

error(['JSimon:', mfilename, ':NoMex'], ...
   'Cannot find compiled Mex.\nPlease compile at first:  mex -O DGradient.c');


function w = compute(w);

%COMPUTE calculates the fast fourier transform of a waveform object
% WAVEFORM = COMPUTE(WAVEFORM) calculate frequency spectrum of a waveform.
% The results of the fast fourier transform is added as new fields
% in the waveform:
%   FFT_FREQ is a vector of frequencies with N samples
%   FFT_AMP is a vector of spetral amplitudes
%   FFT_PHASE is a vector of phases
%   FFT_DOM is the scalar frequency of the maximum amplitude
%           peak (or dominant frequency. This could change)

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
% $Date$
% $Revision$

% CHECK ARGUMENTS
if ~strcmpi(class(w),'waveform')
    error('First input must be a waveform object');
end

% STEP THROUGH WAVFORMS ADDING NEW FIELDS
[N,M] = size(w);
for i = 1:N*M
    Fn = get(w(i),'NYQ');
    x = get(w(i),'DATA');
    NFFT=2.^(ceil(log(length(x))/log(2)));  % Next highest power of 2
    FFTX=fft(x,NFFT);                       % Take fft, padding with zeros.
    NumUniquePts = ceil((NFFT+1)/2);
    FFTX=FFTX(1:NumUniquePts);              % throw out neg frequencies
    MX=abs(FFTX);                           % Take magnitude of X
    MX=MX*2;                                % Multiply by 2
    MX=MX/length(x);
    PX=phase(FFTX);                           % Take magnitude of X
    f=(0:NumUniquePts-1)*2/NFFT;
    f=f*Fn;
    w(i) = addfield(w(i),'FFT_FREQ',f');
    w(i) = addfield(w(i),'FFT_AMP',MX);
    w(i) = addfield(w(i),'FFT_PHASE',PX);
    a = find(MX == max(MX));
    w(i) = addfield(w(i),'FFT_DOM',f(a));
end;


function R2 = RodriguesConversion(R1)
% Rodrigues - converts a Rodrigues rotation vector to rotation matrix or vice versa.
%
% Usage:
%           R2 = Rodrigues(R1)
%
% Input:
%           R1 : Rodrigues rotation vector (3x1 or 1x3) or rotation matrix (3x3).
%
% Output:
%           R2 : rotation matrix (3x3) or Rodrigues rotation vector (3x1 or 1x3).
%
% This code follows the algorithm given by
% [1] R. Hartley and A. Zisserman "Multiple View Geometry in Computer Vision,"
%     Cambridge, pp.583-585, 2003.
%
% Kim, Daesik
% Intelligent Systems Research Center
% Sungkyunkwan Univ. (SKKU), South Korea
% E-mail  : daesik80@skku.edu
% Homepage: http://www.daesik80.com
%
% July 2008  - Original version.

[r,c] = size(R1);

%% Rodrigues Rotation Vector to Rotation Matrix
if ((r == 3) && (c == 1)) || ((r == 1) && (c == 3))
    wx = [  0   -R1(3)  R1(2);
        R1(3)   0   -R1(1);
        -R1(2)  R1(1)   0   ];

    R1_norm = sqrt(R1(1)^2 + R1(2)^2 + R1(3)^2);

    if (R1_norm < eps)
        R2 = eye(3);
    else
        R2 = eye(3) + sin(R1_norm)/R1_norm*wx + (1-cos(R1_norm))/R1_norm^2*wx^2;
    end

    %% Rotation Matrix to Rodrigues Rotation Vector
elseif (r == 3) && (c == 3)
    w_norm = acos((trace(R1)-1)/2);
    if (w_norm < eps)
        R2 = [0 0 0]';
    else
        R2 = 1/(2*sin(w_norm))*[R1(3,2)-R1(2,3);R1(1,3)-R1(3,1);R1(2,1)-R1(1,2)]*w_norm;
    end
end


function [CartPoint,latLong]=randEllipsoidLoc(N,a,f)
%%RANDELLIPSOIDLOC Generate a random point uniformly distributed on the
%              surface of a reference ellipsoid, such as the WGS-84
%              reference ellipsoid. The point is provided in Cartesian
%              coordinates as well as in terms of latitude and longitude.
%
%INPUTS: N The number of random samples on the reference ellipsoid to draw.
%        a The semi-major axis of the reference ellipsoid. If this argument
%          is omitted, the value in Constants.WGS84SemiMajorAxis is used.
%        f The flattening factor of the reference ellipsoid. If this
%          argument is omitted, the value in Constants.WGS84Flattening is
%          used.
%
%OUTPUTS: CartPoint A 3XN matrix of random Cartesian [x;y;z] points on the
%                   ellipsoid with the given semi-major axis and flattening
%                   factor.
%           latLong A 2XN matrix of ellipsoidal latitude and longitude
%                   corresponding to points in CartPoint.
%
%The problem of generating a uniformly distributed sample on a reference
%ellipsoid is number 28 in Chapter 3.4.1 of [1], where the algorithm to do
%so is given in the back of the book. That algorithm is implemented here,
%where the general ellipsoid formulation has been modified to support the
%typical parameterization in terms of a semimajor axis and a flattening
%factor. The algorithm is a rejection sampling method.
%
%Large flattening factor values can make the algorithm slow.
%
%REFERENCES:
%[1] D. Knuth, The Art of Computer Programming: Seminumerical Algorithms,
%    3rd ed. Reading, MA: Addison-Wesley, 1998, vol. 2.
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3)
    f=Constants.WGS84Flattening;
end

if(nargin<2)
    a=Constants.WGS84SemiMajorAxis;
end

%The equation for a reference ellipsoid is
%(x^2+y^2)/a^2+z^2/(a^2*(1-f)^2)=1.
%The algorithm describe in Knuth's book wants things paramterized in terms
%of c(1)*x^2+c(2)*y^2+c(3)*z^2=1. Thus, the coefficients are
c(1,1)=1/a^2;
c(2,1)=1/a^2;
c(3,1)=1/(a^2*(1-f)^2);

%The c's have to be sorted. This means that the ordering of the components
%in x, y, and z will be wrong, so indices to undo the sorting when saving
%the final result need to be found.
[c,sortIdx]=sort(c,'descend');
unsortIdx=1:3;
unsortIdx=unsortIdx(sortIdx);

n=3;%The number of dimensions.

%Allocate space for the return variables.
CartPoint=zeros(3,N);

for curSamp=1:N
    while(1)
        %First, generate a random point on the unit sphere
        y=randDirVec(3);
        rho=sqrt(sum(c.*y.^2));

        %Find K
        if(n*c(n)>=c(1))
            K=sqrt(c(n)^(n-1));
        else
            K=sqrt(((n+1)/(c(1)+c(n)))^(n+1)*(c(1)*c(n)/n)^n);
        end

        %Generate a uniform random variable U
        U=rand(1);

        if(rho^(n+1)*U<K*sqrt(sum(c.^2.*y.^2)))
            CartPoint(unsortIdx,curSamp)=y/rho;
            break;
        end
    end
end

if(nargout>1)
    point=Cart2Ellipse(CartPoint,[],a,f);
    latLong=point(1:2,:);
end
end

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


function a = toeplitz_pds ( m, n, x, y )

%*****************************************************************************80
%
%% TOEPLITZ_PDS returns the TOEPLITZ_PDS matrix.
%
%  Discussion:
%
%    TOEPLITZ is a Toeplitz matrix that is positive definite symmetric.
%
%  Formula:
%
%    A(I,J) = sum ( 1 <= K <= M ) Y(K) * cos ( 2 * PI * X(K) * (I-J) )
%
%  Example:
%
%    N = 5, M = 5,
%    X = ( -0.0625, - 0.03125, 0.0, 0.03125, 0.0625 ),
%    Y = ( 0.2, 0.2, 0.2, 0.2, 0.2)
%
%    1.000000  0.961866  0.852395  0.685661  0.482843
%    0.961866  1.000000  0.961866  0.852395  0.685661
%    0.852395  0.961866  1.000000  0.961866  0.852395
%    0.685661  0.852395  0.961866  1.000000  0.961866
%    0.482843  0.685661  0.852395  0.961866  1.000000
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is Toeplitz: constant along diagonals.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    A is positive definite or positive semi-definite, depending on
%    the values of X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    George Cybenko, Charles Van Loan,
%    Computing the minimum eigenvalue of a symmetric positive definite
%    Toeplitz matrix,
%    SIAM Journal on Scientific and Statistical Computing,
%    Volume 7, 1986, pages 123-131.
%
%  Parameters:
%
%    Input, integer M, the number of terms of W and X.
%
%    Input, integer N, the order of A.
%
%    Input, real X(M), used to define the matrix.
%
%    Input, real Y(M), a set of positive weights used to define the matrix.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n
      a(i,j) = 0.0;
      for k = 1 : m
        angle = 2.0 * pi * x(k) * ( i - j );
        a(i,j) = a(i,j) + y(k) * cos ( angle );
      end
    end
  end

  return
end


function cg_test ( )

%*****************************************************************************80
%
%% CG_TEST tests CG.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CG_TEST:\n' );
  fprintf ( 1, '  CG uses the Conjugate Gradient \n' );
  fprintf ( 1, '  iterative method to approximate the solution \n' );
  fprintf ( 1, '  of a linear system A * x = b.\n' );

  n = 10;

  A = zeros ( n, n );

  for i = 1 : n
    A(i,i) = 2.0;
  end
  for i = 1 : n-1
    A(i,i+1) = -1;
  end
  for i = 2 : n
    A(i-1,i) = -1;
  end
  x = [ 1 : n ]';
  b = A * x;
  x = ones ( n, 1 );

  M = 0;
  max_it = 10;
  tol = 0.0001;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For this example, the order of the system is N = %d\n', n );
  fprintf ( 1, '  The matrix A is the simple tridiagonal -1, 2, -1.\n' );
  fprintf ( 1, '  The correct solution is x = [ 1, 2, ..., n].\n' );
  fprintf ( 1, '  The right hand side b is determined by computing A * x.\n' );
  fprintf ( 1, '  The exact x is then replaced by a vector of all 1''s for\n' );
  fprintf ( 1, '  use as a starting guess.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Other parameters are set as follows:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The maximum number of steps is %d.\n', max_it );
  fprintf ( 1, '  The error tolerance is %f\n', tol );

  [ x, error_norm, iter, flag ] = cg ( A, x, b, M, max_it, tol );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The CG routine has returned with FLAG = %d\n', flag );
  if ( flag == 0 )
    fprintf ( 1, '  This indicates that the iteration has converged.\n' );
  elseif ( flag == 1 )
    fprintf ( 1, '  This indicates that the iteration has NOT converged.\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of iterations taken was %d\n', iter );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The L2 norm of the error per iteration:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : iter
    fprintf ( 1, '  %4d  %f\n', i, error_norm(i) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The computed solution vector X:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %f\n', i, x(i) );
  end

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

  return
end


function a = conex3_inverse ( n )

%*****************************************************************************80
%
%% CONEX3_INVERSE returns the inverse of the CONEX3 matrix.
%
%  Example:
%
%    N = 5
%
%     1  0  0  0  0
%     1  1  0  0  0
%     2  1  1  0  0
%     4  2  1  1  0
%    -8 -4 -2 -1 -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Alan Cline, RK Rew,
%    A set of counterexamples to three condition number estimators,
%    SIAM Journal on Scientific and Statistical Computing,
%    Volume 4, 1983, pages 602-611.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      if ( i < n )

        if ( j < i )
          a(i,j) = 2.0^(i-j-1);
        elseif ( i == j )
          a(i,j) = 1.0;
        else
          a(i,j) = 0.0;
        end

      elseif ( i == n )

        if ( j < i )
          a(i,j) = - 2.0^(i-j-1);
        else
          a(i,j) = -1.0;
        end

      end

    end
  end

  return
end


function [h,X,Y] = ellipseupdate(h,a,b,x0,y0,phi,lineStyle)
%ELLIPSEDRAW can draw an arbitrary ellipse with given parameters.
%   The properties of that ellipse plot can be customized
%   by setting the ellipse handle.
%
%       hEllipse = ellipsedraw(a,b,x0,y0,phi,lineStyle)
%
%   Input parameters:
%       a           Value of the major axis
%       b           Value of the minor axis
%       x0          Abscissa of the center point of the ellipse
%       y0          Ordinate of the center point of the ellipse
%       phi         Angle between x-axis and the major axis
%       lineStyle   Definition of the plotted line style
%
%   Output:
%       hEllipse    Handle of the ellipse
%
%   Simple usage:
%       ellipsedraw(5,3);
%       ellipsedraw(5,3,'g--');
%       ellipsedraw(5,3,pi/4);
%
%   Complete usage:
%       h = ellipsedraw(5,3,1,-2,pi/4,'r-.');
%       set(h,'LineWidth',2);

% Designed by: Lei Wang, <WangLeiBox@hotmail.com>, 25-Mar-2003.
% Last Revision: 01-Apr-2003.
% Dept. Mechanical & Aerospace Engineering, NC State University.
% Copyright (c)2003, Lei Wang <WangLeiBox@hotmail.com>
%$Revision: 1.1 $  $ 4/1/2003 5:42:24 PM $

if (nargin < 3)||(nargin > 7),
    error('Please see help for INPUT DATA.');

elseif nargin == 3
    x0 = 0;     y0 = 0;
    phi = 0;    lineStyle = 'b-';

elseif nargin == 4
    if ischar(x0) == 1
        lineStyle = x0;
        x0 = 0; y0 = 0;
        phi = 0;
    else
        phi = x0;
        x0 = 0; y0 = 0;
        lineStyle = 'b-';
    end

elseif nargin == 5
    phi = 0;    lineStyle = 'b-';

elseif nargin == 6
    lineStyle = 'b-';
end

theta = [-0.03:0.01:2*pi];

% Parametric equation of the ellipse
%----------------------------------------
 x = a*cos(theta);
 y = b*sin(theta);

% Coordinate transform
%----------------------------------------
 X = cos(phi)*x - sin(phi)*y;
 Y = sin(phi)*x + cos(phi)*y;
 X = X + x0;
 Y = Y + y0;

% Plot the ellipse
%----------------------------------------
set(h,'xdata',X,'ydata',Y);
%axis equal;


function [A,U,V] = regutm(m,n,s)
%REGUTM Test matrix for regularization methods.
%
% [A,U,V] = regutm(m,n,s)
%
% Generates a random m-times-n matrix A such that A*A' and A'*A
% are oscillating.  Hence, in the SVD of A,
%    A = U*diag(s)*V',
% the number of sign changes in U(:,i) and V(:,i) is exactly i-1.
%
% The third argument s specifies the singular values of A.  If not
% present, then s = logspace(0,round(log10(eps)),n).

% Reference: P. C. Hansen, "Test matrices for regularization methods",
% SIAM J. Sci. Comput. 16 (1995), 506--512.

% Per Christian Hansen, IMM, 07/30/97.

% Initialization.
if (nargin==1), n = m; end
if (nargin<3), s = logspace(0,round(log10(eps)),min(m,n)); end

% Special treatment of the case m < n.
if (m < n), [A,V,U] = regutm(n,m,s); A = A'; return, end

% Generate random bidiagonal matrix with nonnegative elements.
if (n < 100), mu = .222*n + .0278*n^2; else mu = 3*n; end
B = abs(diag(randn(n,1)+mu) + diag(randn(n-1,1)+mu,1));

% Compute the SVD of B.
[U,dummy,V] = svd(B); clear dummy

% Repeat if m > n.
if (m > n)
  clear U
  B = abs(diag(randn(m,1)+mu) + diag(randn(m-1,1)+mu,1));
  [U,dummy] = svd(B); clear dummy, U = U(:,1:n);
end

% Compute A.
A = U*diag(s)*V';


function a = hadamard ( m, n )

%*****************************************************************************80
%
%% HADAMARD returns a HADAMARD matrix.
%
%  Definition:
%
%    A Hadamard matrix is a square matrix A of order N, whose entries are
%    only +1's or -1's, with the property that:
%
%      A * A' = N * I.
%
%  Notes:
%
%    A Hadamard matrix must be of order 1, 2, or else a multiple of 4.
%    It is not known whether a Hadamard matrix exists for every multiple
%    of 4.
%
%    The method used here allows the user to request a Hadamard matrix
%    of any rectangular order, M by N.  The algorithm then essentially
%    finds the largest powers of 2 that are less than or equal to M and
%    N, and produces a Hadamard-like matrix in that space, setting the
%    rest of the matrix to 0.  Thus, the matrix returned by this routine
%    is only a Hadamard matrix if M = N = a power of 2.
%
%  Formula:
%
%    The following recursive formula is used to produce a series of
%    Hadamard matrices of increasing size.
%
%    H(0) = [1]
%
%    H(1) = [ H(0)  H(0) ] = [ 1  1]
%           [ H(0) -H(0) ]   [ 1 -1]
%
%    H(2) = [ H(1)  H(1) ] = [ 1  1  1  1]
%           [ H(1) -H(1) ]   [ 1 -1  1 -1]
%                            [ 1  1 -1 -1]
%                            [ 1 -1 -1  1]
%
%    and so on.
%
%  Properties:
%
%    All entries of a Hadamard matrix are either +1 or -1.  Matrices
%    produced by this routine will be +1 or -1 up to a certain row
%    and column, beyond which the entries will be zero.
%
%    The Hadamard matrices produced by this routine have the property
%    that the first row and column are entirely 1's, although this
%    is not a requirement for a Hadamard matrix.
%
%    The matrices produced by this algorithm are (loosely) symmetric,
%    although that is not required for a Hadamard matrix.
%
%    Hadamard matrices exhibit the maximum possible relative growth of pivot
%    elements during Gaussian elimination with complete pivoting.
%
%    The inverse of a Hadamard matrix of order N is A itself,
%    scaled by 1.0/N.  Thus 1.0/sqrt(N) times a Hadamard matrix
%    yields a symmetric matrix which is its own inverse, or
%    "involutional".
%
%    A is integral: int ( A ) = A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Gregory and David Karney,
%    Example 3.14,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, New York, 1969, page 42,
%    LC: QA263.G68.
%
%    William Pratt,
%    Digital Image Processing,
%    John Wiley and Sons, 1978.
%
%    Herbert Ryser,
%    Combinatorial Mathematics,
%    John Wiley and Sons, 1963.
%
%  Parameters:
%
%    Input, integer M, the row order of A.
%
%    Input, integer N, the column order of A.
%
%    Output, real A(M,N), the matrix.
%
  a = zeros ( m, n );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HADAMARD - Fatal error!\n' );
    fprintf ( 1, '  Input value of M = %d\n', m );
    fprintf ( 1, '  but M must be positive.\n' );
    error ( 'HADAMARD - Fatal error!' );
  end

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HADAMARD - Fatal error!\n' );
    fprintf ( 1, '  Input value of N = %d\n', n );
    fprintf ( 1, '  but N must be positive.\n' );
    error ( 'HADAMARD - Fatal error!' );
  end

  a(1,1) = 1.0;

  nn = 1;

  while ( nn < n | nn < m )

    for i = 1 : nn
      for j = 1 : nn

        if ( i <= m & j+nn <= n )
          if ( 2 * nn <= n )
            a(i,j+nn) = a(i,j);
          else
            a(i,j+nn) = 0.0;
          end
        end

        if ( i + nn <= m & j <= n )
          if ( 2 * nn <= m )
            a(i+nn,j) = a(i,j);
          else
            a(i+nn,j) = 0.0;
          end
        end

        if ( i + nn <= m & j + nn <= n )
          if ( 2 * nn <= m & 2 * nn <= n )
            a(i+nn,j+nn) = - a(i,j);
          else
            a(i+nn,j+nn) = 0.0;
          end
        end

      end
    end

    nn = 2 * nn;

  end

  return
end


function qwgw_test03 ( )

%*****************************************************************************80
%
%% TEST03 tests QWGW for the Gegenbauer weight.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2014
%
%  Author:
%
%    John Burkardt
%

%
%  Set the quadrature interval and number of points.
%
  a = -1.0;
  b = +1.0;
  n = 5;
%
%  Set the weight function parameter.
%
  alpha = 0.25;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03:\n' );
  fprintf ( 1, '  Compute points and weights for Gauss quadrature\n' );
  fprintf ( 1, '  with the Gegenbauer weight w(x) = (1-x^2)^alpha.\n' );
  fprintf ( 1, '  Order N = %d\n', n );
  fprintf ( 1, '  ALPHA = %g\n', alpha );
  fprintf ( 1, '  Interval = [%g,%g]\n', a, b );
%
%  Set the recursion coefficients.
%
  aj = zeros ( n, 1 );
  bj = zeros ( n, 1 );

  aj(1:n) = 0.0;

  for j = 1 : n - 1
    jr = j;
    bj(j) = ( jr * ( 2.0 * alpha + jr ) ) ...
      / ( 4.0 * ( alpha + jr )^2 - 1.0 );
  end
  bj(n) = 0.0;

  bj(1:n) = sqrt ( bj(1:n) );

  mu0 = gamma ( alpha + 1.0 ) * gamma ( 0.5 ) / gamma ( alpha + 1.5 );
%
%  Compute the points and weights.
%
  [ x, w ] = sgqf ( n, aj, bj, mu0 );

  r8vec_print ( n, x, '  Abscissas:' );
  r8vec_print ( n, w, '  Weights:' );

  return
end


% EX_MAXWELL_EIG_SQUARE: solve Maxwell eigenproblem in the unit square.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_square.txt';

% Type of boundary conditions
problem_data.nmnn_sides   = [];
problem_data.drchlt_sides = [1 2 3 4];

% Physical parameters
problem_data.c_elec_perm = @(x, y) ones(size(x));
problem_data.c_magn_perm = @(x, y) ones(size(x));

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.degree     = [3 3];     % Degree of the bsplines
method_data.regularity = [2 2];     % Regularity of the splines
method_data.nsub       = [10 10];     % Number of subdivisions
method_data.nquad      = [4 4];     % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER
[geometry, msh, space, eigv, eigf] = ...
                             solve_maxwell_eig (problem_data, method_data);

% 4) POSTPROCESSING
[eigv, perm] = sort (eigv);
nzeros = numel (find (eigv < 1e-10));

fprintf ('Number of zero eigenvalues: %i \n', nzeros)
fprintf ('First nonzero eigenvalues: \n')
disp (eigv(nzeros+1:nzeros+5))

figure
sp_plot_solution (eigf(:,perm(nzeros+8)), space, geometry, [30 30])
title ('8^{th} eigenfunction')

%!demo
%! ex_maxwell_eig_square

%!test
%! problem_data.geo_name = 'geo_square.txt';
%! problem_data.nmnn_sides   = [];
%! problem_data.drchlt_sides = [1 2 3 4];
%! problem_data.c_elec_perm = @(x, y) ones(size(x));
%! problem_data.c_magn_perm = @(x, y) ones(size(x));
%! method_data.degree     = [3 3];     % Degree of the bsplines
%! method_data.regularity = [2 2];     % Regularity of the splines
%! method_data.nsub       = [10 10];     % Number of subdivisions
%! method_data.nquad      = [4 4];     % Points for the Gaussian quadrature rule
%! [geometry, msh, space, eigv, eigf] = solve_maxwell_eig (problem_data, method_data);
%! [eigv, perm] = sort (eigv);
%! nzeros = numel (find (eigv < 1e-10));
%! assert (msh.nel, 100)
%! assert (space.ndof, 312)
%! assert (nzeros, 121)
%! assert (eigv(nzeros+(1:5))/pi^2, [1.00000003326399; 1.00000003326400; 2.00000006652799; 4.00000968384168; 4.00000968384168], 2e-14)


function [a3,a2,a1,a0] = createTraj3(theta0,thetaf,thetad0,thetadf,tstart,tfinal)
	% inputs : initial position, velocity + final position, velocity + initial and final times
	% output : a vector specifying the polynomial and can be used with poly functions such as : polyder, polyval, etc.
	% create a 3rd order trajectory
	% example:
	% createTraj3(10,30,0,0,0,1)
	%
	%
	% By: Reza Ahmadzadeh - Matlab/Octave - 2013
	T = tfinal - tstart;
	a0 = theta0;
	a1 = thetad0;
	a2 = (-3 * (theta0 - thetaf) - (2 * thetad0+thetadf )*T)/ T ^ 2;
	a3 = (2 * (theta0 - thetaf) + (thetad0+thetadf )*T)/ T ^ 3;

end


function [ d2, p2 ] = poly_power ( d1, p1, n )

%*****************************************************************************80
%
%% POLY_POWER computes a power of a polynomial.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/triangle_integrals/poly_power.m
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer D1, the degree of the polynomial.
%
%    Input, real P1(M1), the polynomial coefficients.
%    M1 = ((D1+1)*(D1+2))/2.
%
%    Input, integer N, the nonnegative integer power.
%
%    Output, integer D2, the degree of the power polynomial.
%    D2 = N * D1.
%
%    Output, real P2(M2), the polynomial power.
%    M2 = ((D2+1)*(D2+2))/2.
%

%
%  Create P2, a polynomial representation of 1.
%
  d2 = 0;
  m2 = ( ( d2 + 1 ) * ( d2 + 2 ) ) / 2;
  p2 = zeros ( m2, 1 );
  p2(1) = 1.0;
%
%  Iterate N times:
%    P2 <= P2 * P1
%
  for i = 1 : n
    [ d2, p2 ] = poly_product ( d2, p2, d1, p1 );
  end

  return
end


function geometry_test001 ( )

%*****************************************************************************80
%
%% TEST001 tests ANGLE_CONTAINS_POINT_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  ntest = 6;
  n_angle = 12;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST001\n' );
  fprintf ( 1, '  ANGLE_CONTAINS_POINT_2D sees if a point\n' );
  fprintf ( 1, '  lies within an angle.\n' );
  fprintf ( 1, '\n' );
%
%  An acute angle (45 degrees)
%
  for j = 1 : ntest

    if ( j == 1 )

      p1(1:2,1) = [ 1.0; 0.0 ];
      p2(1:2,1) = [ 0.0; 0.0 ];
      p3(1:2,1) = [ 1.0; 1.0 ];

    elseif ( j == 2 )

      p1(1:2,1) = [ 1.0; 0.0 ];
      p2(1:2,1) = [ 0.0; 0.0 ];
      p3(1:2,1) = [ 0.0; 1.0 ];

    elseif ( j == 3 )

      p1(1:2,1) = [ 1.0; -1.0 ];
      p2(1:2,1) = [ 0.0;  0.0 ];
      p3(1:2,1) = [ 0.0;  1.0 ];

    elseif ( j == 4 )

      p1(1:2,1) = [  1.0; 0.0 ];
      p2(1:2,1) = [  0.0; 0.0 ];
      p3(1:2,1) = [ -1.0; 0.0 ];

    elseif ( j == 5 )

      p1(1:2,1) = [ 1.0;  0.0 ];
      p2(1:2,1) = [ 0.0;  0.0 ];
      p3(1:2,1) = [ 0.0; -1.0 ];

    elseif ( j == 6 )

      p1(1:2,1) = [ 1.0;  0.0 ];
      p2(1:2,1) = [ 0.0;  0.0 ];
      p3(1:2,1) = [ 1.0; -0.01 ];

    end

    r8vec_print ( 2, p1, '  Vertex P1' )
    r8vec_print ( 2, p2, '  Vertex P2' )
    r8vec_print ( 2, p3, '  Vertex P3' )

    fprintf ( 1, '\n' );
    fprintf ( 1, '       X            Y       Inside?\n' );
    fprintf ( 1, '\n' );

    for i = 0 : n_angle

      thetar = i * 2.0 * pi / n_angle;
%
%  For some bizarre MATLAB peculiarity, I can't say
%    p(1:2) = [ cos ( thetar ), sin ( thetar ) ];
%  MATLAB (Student 6.5) complains that COS has the wrong number
%  of arguments.  But write it out like this, and magically, everything
%  is fine.  People think I'm crabby for no reason...
%
      p(1,1) = cos ( thetar );
      p(2,1) = sin ( thetar );

      inside = angle_contains_point_2d ( p1, p2, p3, p );

      fprintf ( 1, '  %12f  %12f  %1d\n', p(1:2,1), inside );

    end

  end

  return
end


function h = scattercloud(x,y,n,l,clm,cmap)
%SCATTERCLOUD display density of scatter data
%   SCATTERCLOUD(X,Y) creates a scatterplot of X and Y, displayed over a
%   surface representing the smoothed density of the points.  The density is
%   determined with a 2D histogram, using 25 equally spaced bins in both
%   directions.
%   SCATTERCLOUD(X,Y,N) uses N equally spaced bins.
%   SCATTERCLOUD(X,Y,N,L) uses L as a parameter to the smoothing algorithm.
%    Defaults to 1.  Larger values of L lead to a smoother density, but a
%    worse fit to the original data.
%   SCATTERCLOUD(X,Y,N,L,CLM) uses CLM as the color/linestyle/marker for
%    the scatter plot.  Defaults to 'k+'.
%   SCATTERCLOUD(X,Y,N,L,CLM,CMAP) uses CMAP as the figure's colormap.  The
%    default is 'flipud(gray(256))'.
%   H = SCATTERCLOUD(...) returns the handles for the surface and line
%    objects created.
%
%   Example:
%
%     scattercloud(1:100 + randn(1,100), sin(1:100) + randn(1,100),...
%                  50,.5,'rx',jet(256))
%
%   References:
%     Eilers, Paul H. C. & Goeman, Jelle J. (2004). Enhancing scatterplots
%   with smoothed densities. Bioinformatics 20(5), 623-628.

error(nargchk(2,6,nargin),'struct');

x = x(:);
y = y(:);

if length(x) ~= length(y)
    error('SCATTERCLOUDDataVectorSizesDoNotMatch','The number of elements in x and y do not match')
end

if nargin < 6
    cmap = flipud(gray(256));
end

if nargin < 5
    clm = 'k+';
end

if nargin < 4
    l = 1;
end

if nargin < 3
    n = 25;
end

% min/max of x and y
minX = min(x);
maxX = max(x);
minY = min(y);
maxY = max(y);

% edge locations
xEdges = linspace(minX,maxX,n);
yEdges = linspace(minY,maxY,n);

% shift edges
xDiff = xEdges(2) - xEdges(1);
yDiff = yEdges(2) - yEdges(1);
xEdges = [-Inf, xEdges(2:end) - xDiff/2, Inf];
yEdges = [-Inf, yEdges(2:end) - yDiff/2, Inf];

% number of edges
numX = numel(xEdges);
numY = numel(yEdges);

% hold counts
C = zeros(numY,numX);

% do counts
for i = 1:numY-1
    for j = 1:numX-1
        C(i,j) = length(find(x >= xEdges(j) & x < xEdges(j+1) &...
                             y >= yEdges(i) & y < yEdges(i+1)));
    end
end

% get rid of Infs from the edges
xEdges = [xEdges(2) - xDiff,xEdges(2:end-1), xEdges(end-1) + xDiff];
yEdges = [yEdges(2) - yDiff,yEdges(2:end-1), yEdges(end-1) + yDiff];

% smooth the density data, in both directions.
C = localSmooth(localSmooth(C,l)',l)';

% create the graphics
ax = newplot;
s = surf(xEdges,yEdges,zeros(numY,numX),C,...
         'EdgeColor','none',...
         'FaceColor','interp');
view(ax,2);
colormap(ax,cmap);
grid(ax,'off');
holdstate = get(ax,'NextPlot');
set(ax,'NextPlot','add');
p = plot(x,y,clm);
axis(ax,'tight');
set(ax,'NextPlot',holdstate)

% outputs
if nargout
    h = [s;p];
end

function B = localSmooth(A,L)
r = size(A,1);
I = eye(r);
D1 = diff(I);
D2 = diff(I,2);
B = (I + L ^ 2 * D2' * D2 + 2 * L * D1' * D1) \ A;


clear all, close all, clc

t = (-3:.01:3)';

Utrue = [cos(17*t).*exp(-t.^2) sin(11*t)];
Strue = [2 0; 0 .5];
Vtrue = [sin(5*t).*exp(-t.^2) cos(13*t)];

X = Utrue*Strue*Vtrue';
figure, imshow(X);

%%
sigma = 1;
Xnoisy = X+sigma*randn(size(X));
figure, imshow(Xnoisy);

%%
[U,S,V] = svd(Xnoisy);

N = size(Xnoisy,1);
cutoff = (4/sqrt(3))*sqrt(N)*sigma; % Hard threshold
r = max(find(diag(S)>cutoff)); % Keep modes w/ sig > cutoff
Xclean = U(:,1:r)*S(1:r,1:r)*V(:,1:r)';
figure, imshow(Xclean)

%%
cdS = cumsum(diag(S))./sum(diag(S));  % Cumulative energy
r90 = min(find(cdS>0.90));  % Find r to capture 90% energy

X90 = U(:,1:r90)*S(1:r90,1:r90)*V(:,1:r90)';
figure, imshow(X90)

%% plot singular values

semilogy(diag(S),'-ok','LineWidth',1.5), hold on, grid on
semilogy(diag(S(1:r,1:r)),'or','LineWidth',1.5)
plot([-20 N+20],[cutoff cutoff],'r--','LineWidth',2)
axis([-10 610 .003 300])
rectangle('Position',[-5,20,100,200],'LineWidth',2,'LineStyle','--')

figure
semilogy(diag(S),'-ok','LineWidth',1.5)
hold on, grid on
semilogy(diag(S(1:r,1:r)),'or','LineWidth',1.5)
plot([-20 N+20],[cutoff cutoff],'r--','LineWidth',2)
axis([-5 100 20 200])

figure
plot(cdS,'-ok','LineWidth',1.5)
hold on, grid on
plot(cdS(1:r90),'ob','LineWidth',1.5)
plot(cdS(1:r),'or','LineWidth',1.5)
set(gca,'XTick',[0 300 r90 600],'YTick',[0 .5 0.9 1.0])
xlim([-10 610])
plot([r90 r90 -10],[0 0.9 0.9],'b--','LineWidth',1.5)


function diftab = dif_basis ( ntab, xtab )

%*****************************************************************************80
%
%% DIF_BASIS computes all Lagrange basis polynomials in divided difference form.
%
%  Discussion:
%
%    The I-th Lagrange basis polynomial for a set of NTAB X values XTAB,
%    L(I,NTAB,XTAB)(X) is a polynomial of order NTAB-1 which is zero at
%    XTAB(J) for J not equal to I, and 1 when J is equal to I.
%
%    The Lagrange basis polynomials have the property that the interpolating
%    polynomial through a set of NTAB data points (XTAB,YTAB) may be
%    represented as
%
%      P(X) = Sum ( 1 <= I <= N ) YTAB(I) * L(I,NTAB,XTAB)(X)
%
%    Higher order interpolation at selected points may be accomplished
%    using repeated X values, and scaled derivative values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carl deBoor,
%    A Practical Guide to Splines,
%    Springer, 2001,
%    ISBN: 0387953663,
%    LC: QA1.A647.v27.
%
%  Parameters:
%
%    Input, integer NTAB, the number of X data points XTAB, and the number of
%    basis polynomials to compute.
%
%    Input, real XTAB(NTAB), the X values upon which the
%    Lagrange basis polynomials are to be based.
%
%    Output, real DIFTAB(NTAB,NTAB), the set of divided
%    difference tables.  Column I of DIFTAB contains the table for
%    the I-th Lagrange basis polynomial.
%

%
%  Initialize DIFTAB to the identity matrix.
%
  diftab(1:ntab,1:ntab) = 0.0;
  for i = 1 : ntab
    diftab(i,i) = 1.0;
  end
%
%  Compute each Lagrange basis polynomial.
%
  for i = 1 : ntab
    diftab(1:ntab,i) = ( data_to_dif ( ntab, xtab, diftab(1:ntab,i) ) )';
  end

  return
end


function c = r8ut_mxm ( n, a, b )

%*****************************************************************************80
%
%% R8UT_MXM multiplies two R8UT matrices.
%
%  Discussion:
%
%    The R8UT storage format is used for an M by N upper triangular
%    matrix.  The format stores all M*N entries, even those which are zero.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrices.
%    N must be positive.
%
%    Input, real A(N,N), B(N,N), the R8UT factor matrices.
%
%    Output, real C(N,N), the R8UT product matrix.
%
  c = zeros ( n, n );

  for i = 1 : n
   for j = i : n
      for k = i : j
        c(i,j) = c(i,j) + a(i,k) * b(k,j);
      end
    end
  end

  return
end


function [X12,x1,x2] = DigitalOscillator(f1,f2,Fs,t1)

% Function to simulate Digital Sinusodial Oscillator
% Input Variables
%    f1 = first input analog frequency    Suggested Range(697-1477)
%    f2 = second input analog frequency   Suggested Range(697-1477
%    Fs = Sampling Frequency              Default = 8000 cycles/sec
%    t1 = Tone Length                     Default = .1 sec
%
% Output Variables
%    Y  = Sine Signal for two given frequencies
%    x1 = Sine Signal for first frequency
%    x2 = Sine Signal for second frequency
%
%    Rajiv Singla        DSP Final Project            Fall 2005
%=====================================================================

% Checking for minimum number of arguments
if nargin < 2
    error('Not enough input arguments');
end

% Setting Default values
if nargin == 2
    Fs = 8000;
    t1 = 0.1;
end

Ts=1/Fs;   %Sampling Time
samples=t1/Ts;  %Number of samples in the tone

%% Generating first Signal
w0=2*3.1416*f1/Fs;
A=1;
a1=-2*cos(w0);
Y1=0;
Y2=-A*sin(w0);

for n=1:samples
    x1(n)=-a1*Y1-Y2;
    Y2=Y1;
    Y1=x1(n);
end
x1=[0 x1]; %appending zero in the beginning

%% Generating second Signal
w1=2*3.1416*f2/Fs;
A=1;
a2=-2*cos(w1);
P1=0;
P2=-A*sin(w1);

for n=1:samples
    x2(n)=-a2*P1-P2;
    P2=P1;
    P1=x2(n);
end
x2=[0 x2]; %appending zero in the beginning

%% Combining both signal
X12 = .5*(x1 + x2);


function value = pyramid_unit_monomial_3d ( alpha, beta, gamma )

%*****************************************************************************80
%
%% PYRAMID_UNIT_MONOMIAL_3D: monomial integral in a unit pyramid in 3D.
%
%  Discussion:
%
%    This routine returns the integral of X^ALPHA Y^BETA Z^GAMMA over
%    the unit pyramid.
%
%    The unit pyramid is defined as:
%
%    - ( 1 - Z ) <= X <= 1 - Z
%    - ( 1 - Z ) <= Y <= 1 - Z
%              0 <= Z <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 April 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer ALPHA, BETA, GAMMA, the exponents of
%    X, Y and Z in the monomial.
%
%    Output, real PYRAMID_UNIT_MONOMIAL_3D, the volume of the pyramid.
%
  value = 0.0;

  if ( mod ( alpha, 2 ) == 0 & mod ( beta, 2 ) == 0 )

    i_hi = 2 + alpha + beta;

    for i = 0 : i_hi
      value = value + r8_mop ( i ) * r8_choose ( i_hi, i ) / ( i + gamma + 1 );
    end

    value = value * 2.0 / ( alpha + 1 ) * 2.0 / ( beta + 1 );

  end

  return
end


function divdif_test20 ( )

%*****************************************************************************80
%
%% DIVDIF_TEST20 tests DIF_DERIVK_TABLE;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'DIVDIF_TEST20\n' );
  fprintf ( 1, '  For a divided difference polynomial:\n' );
  fprintf ( 1, '  DIF_DERIVK_TABLE computes the K-th derivative;\n' );
%
%  Set the 0 data points.
%
  n0 = 5;
  x0 = linspace ( -2.0, +2.0, 5 );
%
%  Set data for x^4/24+x^3/3+x^2/2+x+1
%
  f0(1:n0) = 1.0;
  for i = 4 : -1 : 1
    f0(1:n0) = f0(1:n0) .* x0(1:n0) / i + 1.0;
  end
%
%  Compute the difference table.
%
  d0 = data_to_dif ( n0, x0, f0 );
  dif_print ( n0, x0, d0, '  The divided difference polynomial P0:' );

  c0 = dif_to_r8poly ( n0, x0, d0 );

  r8poly_print ( n0, c0, '  Using DIF_TO_R8POLY' );
%
%  Compute the difference table for the K=1 derivative.
%
  k = 1;
  n1 = n0 - k;
  [ x1, d1 ] = dif_derivk_table ( n0, x0, d0, k );
%
%  Compute the difference table for the K=2 derivative.
%
  k = 2;
  n2 = n0 - k;
  [ x2, d2 ] = dif_derivk_table ( n0, x0, d0, k );
%
%  Compute the difference table for the K=3 derivative.
%
  k = 3;
  n3 = n0 - k;
  [ x3, d3 ] = dif_derivk_table ( n0, x0, d0, k );
%
%  Compute the difference table for the K=4 derivative.
%
  k = 4;
  n4 = n0 - k;
  [ x4, d4 ] = dif_derivk_table ( n0, x0, d0, k );
%
%  Evaluate all 5 polynomials.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Evaluate difference tables for the function P0\n' );
  fprintf ( 1, '  and its first four derivatives, P1...P4.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X         P0        P1        P2        P3        P4\n' );
  fprintf ( 1, '\n' );

  for i = 0 : 10
    x = i / 5.0;
    y0 = dif_val ( n0, x0, d0, x );
    y1 = dif_val ( n1, x1, d1, x );
    y2 = dif_val ( n2, x2, d2, x );
    y3 = dif_val ( n3, x3, d3, x );
    y4 = dif_val ( n4, x4, d4, x );
    fprintf ( 1, '  %8.4f  %8.4f  %8.4f  %8.4f  %8.4f  %8.4f\n', ...
      x, y0, y1, y2, y3, y4 );
  end

  return
end


function varargout = PoissonRatio(C,varargin)
% computes the Poisson ratio of an elasticity tensor
%
% Input
%  C - elastic @stiffnessTensor
%  x - @vector3d
%  y - @vector3d
%
% Output
%  nu - Poisson ratio in directions x and y
%
% Description
%
% $$\nu = \frac{-S_{ijkl} x_i x_j y_k y_l}{S_{mnop} x_m x_n x_o x_p}$$
%

% take formula using complience
[varargout{1:nargout}] = PoissonRatio(inv(C),varargin{:});


function OUT = MovCorr(X,window,i)
% =======================================================================
% Computes correlation between X_i and X_j for all j different
% of i, from window to the number of observations.  The
% first window rows are NaN.
% =======================================================================
% OUT = MovCorr(X, window, i)
% -----------------------------------------------------------------------
% INPUT
%   - X: panel time series T observations x N variables
%   - window : size of the moving window
%   - i: the variable X_i against which correlations should be returned
% -----------------------------------------------------------------------
% OUTPUT
%   - OUT: matrix with moving correlation T observations x N-1
%       variables. The first window-1 rows are NaN.
% =======================================================================
% EXAMPLE
% X = rand(100,5);
% OUT = MovCorr(X,20,1)
% =======================================================================
% Ambrogio Cesa Bianchi, March 2015

[nobs,nvars] = size(X);
OUT = nan(nobs,nvars-1);

for tt = window:nobs
    C = corrcoef(X(tt-window+1:tt, :));
    idx = setdiff(1:nvars, [i]);
    OUT(tt, :) = C(i, idx);
end


function loc_3d_torus(NumOfSamples)

% Create Localized 3-d data from uniform distribution.
% Data are bount to the neighborhood near a torus surface.
% Torus surface equation:
%
%         (c - sqrt(x^2+y^2))^2 + z^2 = a^2
%
% This torus is created by revoloution around z axis of a circle centered
% at (c,0) with radious a.

clc;
input_dims = 3;

a = 1;
a1 = .9;
c = 5;
% Initialize Data
Data = [];

while size(Data,2)<NumOfSamples

In1 = 20*(rand(input_dims,1)-.5);

% Apply Mask to input Data
if abs(In1(3,1))<= sqrt(a^2 - (c-sqrt(In1(1,1)^2 + In1(2,1)^2))^2) && ...
                                                                       abs(In1(3,1))>=sqrt(a1^2 - (c-sqrt(In1(1,1)^2 + In1(2,1)^2))^2)
    Data = [Data In1];
end
end

a = randperm(NumOfSamples);
Data = Data(:,a);

save local_torus Data;
plot3(Data(1,:),Data(2,:),Data(3,:),'.','MarkerSize',1);
grid on;
clear all;


function matreg = Tikhonov_rank_def(mat, rank, lambda)

% Apply Tikhonov regularisation to rank-deficient matrix

% mat: square matrix

% rank: number of singular values to be considered in inversion

% lambda: regularisation parameters

% OH, Sep 2018

% SVD of input matrix

[U,S,V] = svd(mat);

% get singular values

s = diag(S);

% take only relevant values

s2 = s(1:rank);

lambda = (lambda/100) * (sum(s2)/rank);

% regularise eigenvalues with Tikhonov and invert

s2 = s2 ./ (s2.^2 + lambda);

% reconstitute regularised inverse matrix

matreg = V(:,1:rank)*diag(s2)*U(:,1:rank)';


function [t,u,v,idx]=raytrace(p0,v0,node,face)
%
% [t,u,v,idx]=raytrace(p0,v0,node,face)
%
% perform a Havel-styled ray tracing for a triangular surface
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%   p0: starting point coordinate of the ray
%   v0: directional vector of the ray
%   node: a list of node coordinates (nn x 3)
%   face: a surface mesh triangle list (ne x 3)
%
% output:
%   t: signed distance from p to the intersection point for each surface
%      triangle, if ray is parallel to the triangle, t is set to Inf
%   u: bary-centric coordinate 1 of all intersection points
%   v: bary-centric coordinate 2 of all intersection points
%      the final bary-centric triplet is [u,v,1-u-v]
%   idx: optional output, if requested, idx lists the IDs of the face
%      elements that intersects the ray; users can manually calc idx by
%
%      idx=find(u>=0 & v>=0 & u+v<=1.0 & ~isinf(t));
%
% Reference:
%  [1] J. Havel and A. Herout, "Yet faster ray-triangle intersection (using
%          SSE4)," IEEE Trans. on Visualization and Computer Graphics,
%          16(3):434-438 (2010)
%  [2] Q. Fang, "Comment on 'A study on tetrahedron-based inhomogeneous
%          Monte-Carlo optical simulation'," Biomed. Opt. Express, (in
%          press)
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

p0=p0(:)';
v0=v0(:)';

AB=node(face(:,2),1:3)-node(face(:,1),1:3);
AC=node(face(:,3),1:3)-node(face(:,1),1:3);

N=cross(AB',AC')';
d=-dot(N',node(face(:,1),1:3)')';

Rn2=1./sum((N.*N)')';

N1=cross(AC',N')'.*repmat(Rn2,1,3);
d1=-dot(N1',node(face(:,1),1:3)')';

N2=cross(N',AB')'.*repmat(Rn2,1,3);
d2=-dot(N2',node(face(:,1),1:3)')';

den=(v0*N')';
t=-(d+(p0*N')');
P=(p0'*den'+v0'*t')';
u=dot(P',N1')'+den.*d1;
v=dot(P',N2')'+den.*d2;

idx=find(den);
den(idx)=1./den(idx);

t=t.*den;
u=u.*den;
v=v.*den;

 % if den==0, ray is parallel to triangle, set t to infinity
t(find(den==0))=Inf;

if(nargout>=4)
    idx=find(u>=0 & v>=0 & u+v<=1.0 & ~isinf(t));
end


function mean = discrete_mean ( a, b )

%*****************************************************************************80
%
%% DISCRETE_MEAN evaluates the mean of the Discrete PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer A, the number of probabilities assigned.
%
%    Input, real B(A), the relative probabilities of
%    outcomes 1 through A.  Each entry must be nonnegative.
%
%    Output, real MEAN, the mean of the PDF.
%
  b_sum = sum ( b(1:a) );

  mean = 0.0;
  for j = 1 : a
    mean = mean + j * b(j);
  end

  mean = mean / b_sum;

  return
end


% Maximum volume inscribed ellipsoid in a polyhedron
% Section 8.4.1, Boyd & Vandenberghe "Convex Optimization"
% Original version by Lieven Vandenberghe
% Updated for CVX by Almir Mutapcic - Jan 2006
% (a figure is generated)
%
% We find the ellipsoid E of maximum volume that lies inside of
% a polyhedra C described by a set of linear inequalities.
%
% C = { x | a_i^T x <= b_i, i = 1,...,m } (polyhedra)
% E = { Bu + d | || u || <= 1 } (ellipsoid)
%
% This problem can be formulated as a log det maximization
% which can then be computed using the det_rootn function, ie,
%     maximize     log det B
%     subject to   || B a_i || + a_i^T d <= b,  for i = 1,...,m

% problem data
n = 2;
px = [0 .5 2 3 1];
py = [0 1 1.5 .5 -.5];
m = size(px,2);
pxint = sum(px)/m; pyint = sum(py)/m;
px = [px px(1)];
py = [py py(1)];

% generate A,b
A = zeros(m,n); b = zeros(m,1);
for i=1:m
  A(i,:) = null([px(i+1)-px(i) py(i+1)-py(i)])';
  b(i) = A(i,:)*.5*[px(i+1)+px(i); py(i+1)+py(i)];
  if A(i,:)*[pxint; pyint]-b(i)>0
    A(i,:) = -A(i,:);
    b(i) = -b(i);
  end
end

% formulate and solve the problem
cvx_begin
    variable B(n,n) symmetric
    variable d(n)
    maximize( det_rootn( B ) )
    subject to
       for i = 1:m
           norm( B*A(i,:)', 2 ) + A(i,:)*d <= b(i); %#ok
       end
cvx_end

% make the plots
noangles = 200;
angles   = linspace( 0, 2 * pi, noangles );
ellipse_inner  = B * [ cos(angles) ; sin(angles) ] + d * ones( 1, noangles );
ellipse_outer  = 2*B * [ cos(angles) ; sin(angles) ] + d * ones( 1, noangles );

clf
plot(px,py)
hold on
plot( ellipse_inner(1,:), ellipse_inner(2,:), 'r--' );
plot( ellipse_outer(1,:), ellipse_outer(2,:), 'r--' );
axis square
axis off
hold off


function geometry_test012 ( )

%*****************************************************************************80
%
%% TEST012 tests CIRCLE_LUNE_AREA_2D, CIRCLE_SECTOR_AREA_2D, CIRCLE_TRIANGLE_AREA_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;
  n_test = 12;

  center(1:2) = [ 0.0, 0.0 ];
  r = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST012\n' );
  fprintf ( 1, '  CIRCLE_LUNE_AREA_2D computes the area of a\n' );
  fprintf ( 1, '    circular lune, defined by joining the endpoints\n' );
  fprintf ( 1, '    of a circular arc.\n' );
  fprintf ( 1, '  CIRCLE_SECTOR_AREA_2D computes the area of a\n' );
  fprintf ( 1, '    circular sector, defined by joining the endpoints\n' );
  fprintf ( 1, '    of a circular arc to the center.\n' );
  fprintf ( 1, '  CIRCLE_TRIANGLE_AREA_2D computes the signed area of a\n' );
  fprintf ( 1, '    triangle, defined by joining the endpoints\n' );
  fprintf ( 1, '    of a circular arc and the center.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '      R            Theta1      Theta2        Sector       Triangle     Lune\n' );
  fprintf ( 1, '\n' );

  for i = 0 : n_test

    theta1 = 0.0;
    theta2 = i * 2.0 * pi / n_test;

    area1 = circle_sector_area_2d ( r, center, theta1, theta2 );

    area2 = circle_triangle_area_2d ( r, center, theta1, theta2 );

    area3 = circle_lune_area_2d ( r, center, theta1, theta2 );

    fprintf ( 1, '  %10f  %10f  %10f  %10f  %10f  %10f\n', ...
      r, theta1, theta2, area1, area2, area3 );

  end

  return
end


function fx = p42_fun ( n, x )

%*****************************************************************************80
%
%% P42_FUN evaluates the integrand for problem 42.
%
%  Discussion:
%
%    The problem has a parameter ALPHA that can be set by calling
%    P42_PARAM_SET.
%
%    The integrand has a singularity at X = 0 if ALPHA < 1.
%
%    The suggested range for ALPHA is 0.1 through 2.
%
%  Interval:
%
%    0 <= x <= pi/2
%
%  Integrand:
%
%    ( sin(x) )^( alpha - 1 )
%
%  Exact Integral:
%
%    2^( alpha - 2 ) * ( Gamma(alpha/2) )^2 / Gamma(alpha)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Piessens, Elise de Doncker-Kapenga,
%    Christian Ueberhuber, David Kahaner,
%    QUADPACK: A Subroutine Package for Automatic Integration,
%    Springer, 1983, page 84.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the integrand values.
%
  alpha = p42_param_get ( );

  fx = sin ( x ).^( alpha - 1.0 );

  if ( alpha < 1.0 )
    i = find ( x == 0.0 );
    fx(i) = 0.0;
  end

  return
end


function c=addlogs(a,b)
%ADDLOGS Add numbers represented by their logarithms.
%
%  Description
%    C=ADDLOGS(A,B) computes C=log(exp(A)+exp(B)) in such a fashion
%    that it works even when A and B have large magnitude.

% Copyright (c) 2003 Aki Vehtari

% 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 a>b
  c = a + log(1+exp(b-a));
else
  c = b + log(1+exp(a-b));
end


function [Gausspoint,Gaussweight] = GaussQuadrature(ngl)
%-------------------------------------------------------------------
%  Purpose:
%     determine the integration points and weighting coefficients
%     of Gauss-Legendre quadrature for two-dimensional integration
%
%  Synopsis:
%     [point,weight]=GaussQuadrature(nglx,ngly)
%
%  Variable Description:
%     ngl - number of integration points
%     point - vector containing integration points
%     weight - vector containing weighting coefficients
%-------------------------------------------------------------------
%  initialization

   Gausspoint=zeros(ngl,1);
   Gaussweight=zeros(ngl,1);

%  corresponding integration points and weights
    % 2-point quadrature rule
    Gausspoint(1)=-0.577350269189626;
    Gausspoint(2)=-Gausspoint(1);
    Gaussweight(1)=1.0;
    Gaussweight(2)=Gaussweight(1);


function [ R ] = R_rpy( roll, pitch, yaw )
%R_RPY この関数の概要をここに記述
%   Camera rotates (1) yaw (y-axis) (2)pitch (x'-axis) (3)roll (z''-axis)

rrad = -roll * pi / 180.0;
prad = -pitch * pi / 180.0;
yrad = -yaw * pi / 180.0;

Rr = [cos(rrad), -sin(rrad), 0;...
      sin(rrad), cos(rrad), 0;...
      0, 0, 1];

Rp = [1, 0, 0;...
      0, cos(prad), -sin(prad);...
      0, sin(prad), cos(prad)];

Ry = [cos(yrad), 0, sin(yrad);...
      0, 1, 0;...
      -sin(yrad), 0, cos(yrad)];

R = Rr * Rp * Ry;

end


function s = CubicTimeScaling(Tf, t)
% *** CHAPTER 9: TRAJECTORY GENERATION ***
% Takes Tf: Total time of the motion in seconds from rest to rest,
%       t: The current time t satisfying 0 < t < Tf.
% Returns s: The path parameter s(t) corresponding to a third-order
%            polynomial motion that begins and ends at zero velocity.
% Example Input:
%
% clear; clc;
% Tf = 2;
% t = 0.6;
% s = CubicTimeScaling(Tf,t)
%
% Output:
% s =
%    0.2160

s = 3 * (t / Tf) ^ 2 - 2 * (t / Tf) ^ 3;
end


function transformationMatrix = create_transformation_matrix3d(varargin)
% CREATE_TRANSFORMATION_MATRIX3D Create a 3D transformation matrix
%
% [transformationMatrix rotationMatrix shearMatrix scaleMatrix] = ...
%   create_transformation_matrix3d()
%
% INPUT ARGUMENTS
% N/A
%
% Optional input arguments
% 'phiX'            - Rotation angle around x-axis
% 'phiY'            - Rotation angle around y-axis
% 'phiZ'            - Rotation angle around z-axis
% 'shearXY'         - Shear XY
% 'shearXZ'         - Shear XZ
% 'shearYX'         - Shear YX
% 'shearYZ'         - Shear YZ
% 'shearZX'         - Shear ZX
% 'shearZY'         - Shear ZY
% 'scaleX'          - Scale along x-axis
% 'scaleY'          - Scale along y-axis
% 'scaleZ'          - Scale along z-axis
% 'translationX'    - Translation along x-axis
% 'translationY'    - Translation along y-axis
% 'translationZ'    - Translation along z-axis
%
% OUTPUT ARGUMENTS
% transformationMatrix  - Total transformation matrix
% rotationMatrix        - Rotation matrix
% shearMatrix           - Shear matrix
% scaleMatrix           - Scale matrix

% Copyright (c) 2012 Daniel Forsberg
% danne.forsberg@outlook.com
%
% 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/>.

phiX = 0;
phiY = 0;
phiZ = 0;
shearXY = 0;
shearXZ = 0;
shearYX = 0;
shearYZ = 0;
shearZX = 0;
shearZY = 0;
scaleX = 1;
scaleY = 1;
scaleZ = 1;
translationX = 0;
translationY = 0;
translationZ = 0;

for k=1:2:length(varargin),         % overwrites default parameter
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

rotationX = [1 0         0;
             0 cos(phiX) -sin(phiX);
             0 sin(phiX) cos(phiX)];
rotationY = [cos(phiY)  0 sin(phiY);
             0          1 0;
             -sin(phiY) 0 cos(phiY)];
rotationZ = [cos(phiZ) -sin(phiZ) 0;
             sin(phiZ) cos(phiZ)  0;
             0         0          1];

shearMatrix = [1       shearXY shearXZ;
               shearYX 1       shearYZ;
               shearZX shearZY 1];

scaleMatrix = [scaleX 0      0;
               0      scaleY 0;
               0      0      scaleZ];

transformationMatrix = zeros(3,4);
transformationMatrix(1:3,1:3) = rotationX*rotationY*rotationZ*shearMatrix*scaleMatrix;
transformationMatrix(:,4) = [translationX translationY translationZ]';


function [X_poly] = polyFeatures(X, p)
%POLYFEATURES Maps X (1D vector) into the p-th power
%   [X_poly] = POLYFEATURES(X, p) takes a data matrix X (size m x 1) and
%   maps each example into its polynomial features where
%   X_poly(i, :) = [X(i) X(i).^2 X(i).^3 ...  X(i).^p];
%

% You need to return the following variables correctly.
X_poly = zeros(numel(X), p);

% ====================== YOUR CODE HERE ======================
% Instructions: Given a vector X, return a matrix X_poly where the p-th
%               column of X contains the values of X to the p-th power.
%
%

for i = 1: p
  X_poly(:, i) = X' .^i;
end

% =========================================================================

end


function [p,c,v] = matrixpolynomial(x,n,dmax,dmin)
%MATRIXPOLYNOMIAL Creates parameterized polynomial
%
% [p,c,v] = matrixpolynomial(x,n,dmax,dmin)
%
% MATRIXPOLYNOMIAL is a quick way to define a parameterized polynomial
% p=sum C_i v_i(x) with all monomials of dmin <= degree(p,x) <= dmax. The
% coefficients in the polynomial are C_i while v is the monomial basis.
%
% Example:
%
% Paramterized quartic 2x2 matrix
%  x = sdpvar(2,1);
%  p = matrixpolynomial(x,2,4);
%
% See also MONOLIST, COEFFICIENTS

if (length(dmax) > 1) && (length(dmax) ~= length(x))
    error('Dimension mismatch: The third argument should be the max degree for each variable, or a sclar');
end

if nargin > 3
    if (length(dmin) > 1) && (length(dmin) ~= length(x))
        error('Dimension mismatch: The third argument should be the max degree for each variable, or a sclar');
    end
end

if any(dmax < 0)
    error('Only non-negative polynomial degrees possible')
end

if nargin<4
    dmin = 0;
end

if any(dmin > dmax)
    error('Fourth argument (dmin) should not be larger than second argument (dmax)');
end

if any(dmin < 0)
    error('Only non-negative polynomial degrees possible')
end

if length(n)==1
    n = [n n];
end

v = monolist(x,dmax);

if dmin <= dmax & dmin>0
    s = nchoosek(length(x) + dmin-1,dmin-1);
    v = extsubsref(v,s+1:length(v));
end

p = 0;
c = [];
for i = 1:length(v)
    Ci = sdpvar(n(1),n(2));
    c = [c reshape(Ci,[],1)];
end
p = reshape(c*v,n(1),n(2));


%% Example:
% Compute the tridiagonal matrix used to solve implicitly a laplace (heat)
% equation.

m = 5; % domain size.
h = 0.1; % delta x value.

I = eye(m);
e = ones(1,m);
x = spdiags([e' -4*e' e'],[-1 0 1],m,m);
%x = zeros(m);
%x = x+spdiags([e' -4*e' e'],[-1 0 1],m,m);
s = spdiags([e',e'],[-1 1],m,m);
s = zeros(m);
s = s+spdiags([e',e'],[-1 1],m,m);
%kron(I,x);
%kron(s,I);
A = (kron(I,x)+kron(s,I))/h^2;
%size(A);


% GENERATE BILINEAR TIME-FREQUENCY TRANSFORMATIONS USING THE DIRECT METHOD
%
% BILINEAR TRANSFORMATIONS transform the time domain signal (the
% variable INPUT SIGNAL) to an output time-frequency distribution (the
% variable OUTPUT TIME-FREQUENCY ARRAY)
%
% These functions use a direct implementation rather than using the
% quadratic time-frequency implementation. This results in a
% computationally optimised routine.
%
% The different types of time-frequency distributions that can be
% implemented directly are:
%
%   (i)    Wigner-Ville Distribution
%   (ii)   Short-Time Fourier Transform
%   (iii)  Short-Time Fourier Transform(overlap)
%   (iv)   Rihaczek
%   (v)    Windowed-Rihaczek
%
%   The various parameters associated with the distributions are as follows:
%
%   TIME-FREQUENCY ARRAY (tfd)
%
%      The computed time-frequency distribution.  size(tfd) will
%      return [a, b], where a is the next largest power of two above
%      FFT length, and b is floor(length(signal)/time_res) - 1.
%
%   INPUT SIGNAL
%
%      Input one dimensional signal to be analysed. An analytic signal
%      is required for this function, however, if signal is real, a
%      default analytic transformer routine will be called from this
%      function before computing tfd.
%
%   TIME RESOLUTION
%
%      The number of time samples to skip between successive time slices.
%
%   LAG WINDOW LENGTH
%
%      This is the lag window length and controls the size of the
%      signal kernel (or instantaneous autocorrelation function) used
%      for analysis (lag_window_length must be odd). The kernel used
%      will be defined from -(lag_window_length+1)/2 to
%      +(lag_window_length+1)/2 in both time and lag dimensions.
%
%   FFT Length:
%
%      Zero-padding at the FFT stage of the analysis may be specified
%      by giving an FFT length larger than lag window length.  If
%      FFT length is not specified, or is smaller than the
%      lag window length, then the next highest power of two above
%      lag window length is used.  If FFT length is not a power of
%      two, the next highest power of two is used.
%
%
%
%  See Also:  wvd, spec, rihaczek, analyt


function [c4,m4,c2] = cum4(X,prewhiten)
%CUM4 Fourth-order cumulant tensor.
%   [c4,m4,c2] = cum4(X) computes the second-order cumulant (covariance
%   matrix) c2, fourth-order moment m4 and fourth-order cumulant
%   (quadricovariance tensor) c4 of a matrix X in which each row is an
%   observation and each column is a variable. Herein,
%
%      c2(i,j)     = E[xi.*conj(xj)]
%      m4(i,j,k,l) = E[xi.*conj(xj).*conj(xk).*xl]
%      c4(i,j,k,l) = E[xi.*conj(xj).*conj(xk).*xl] ...
%                    - E[xi.*conj(xj)]*E[conj(xk).*xl] ...
%                    - E[xi.*conj(xk)]*E[conj(xj).*xl] ...
%                    - E[xi.*xl]*E[conj(xj).*conj(xk)]
%
%   where the expectation E is approximated by the arithmetic mean and xi
%   is the i-th mean centered variable, X(:,i)-mean(X(:,i)) (and
%   analogously for xj, xk and xl).
%
%   [c4,m4,c2] = cum4(X,'prewhiten') applies a linear transformation to the
%   columns of X so that the covariance matrix of the new matrix is the
%   identity matrix before computing its fourth-order cumulant.
%
%   See also cov, scov.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] P. McCullagh, "Tensor Methods in Statistics," Chapman and Hall,
%       London, 1987.
%   [2] C. Nikias, A. Petropulu, "Higher-Order Spectra Analysis: A
%       Nonlinear Signal Processing Framework," Prentice Hall, 1993.

% Check the prewhiten option.
if nargin < 3, prewhiten = false; end
if ischar(prewhiten), prewhiten = strcmpi(prewhiten,'prewhiten'); end

% Center the variables.
X = bsxfun(@minus,X,mean(X));

% Apply a prewhitening to X if requested.
n = size(X,1);
if prewhiten
    [U,S,~] = svd(X,'econ');
    X = U*(S*pinv(S))*sqrt(n);
end

% Compute c2 = E[xi*conj(xj)] and r2 = E[xi*xj].
c2 = conj(X'*X)/n;
r2 = (X.'*X)/n;

% Compute m4 = E[xi*conj(xj)*conj(xk)*xl].
% Introduce singleton dimensions 3 and 4.
m4 = bsxfun(@times,permute(X,[2 3 4 5 1]),permute(conj(X),[3 2 4 5 1]));
m4 = mean(bsxfun(@times,m4,permute(m4,[3 4 2 1 5])),5);

% Compute c4(i,j,k,l) = m4 - E[xi.*conj(xj)]*E[conj(xk).*xl] - ...
% E[xi.*conj(xk)]*E[conj(xj).*xl] - E[xi.*xl]*E[conj(xj).*conj(xk)].
c4 = m4-bsxfun(@times,c2,permute(c2,[3 4 2 1])) ...
       -bsxfun(@times,permute(c2,[1 3 2 4]),permute(c2,[3 2 4 1])) ...
       -bsxfun(@times,permute(r2,[1 3 4 2]),permute(conj(r2),[3 1 2 4]));


%% Sequential update for Gaussian
close all; clear;
d = 2;
n = 100;
X = randn(d,n);
x = randn(d,1);

mu = mean(X,2);
Xo = bsxfun(@minus,X,mu);
Sigma = Xo*Xo'/n;
p1 = logGauss(x,mu,Sigma);

gauss = Gauss(X(:,3:end)).addSample(X(:,1)).addSample(X(:,2)).addSample(X(:,3)).delSample(X(:,3));
p2 = gauss.logPdf(x);
maxdiff(p1,p2)
%% Sequential update for Gaussian-Wishart
close all; clear;
d = 2;
n = 100;
X = randn(d,n);
x = randn(d,1);

kappa0 = 1;
m0 = zeros(d,1);
nu0 = d;
S0 = eye(d);

xbar = mean(X,2);
kappa = kappa0+n;
nu = nu0+n;
m = (n*xbar+kappa0*m0)/kappa;
Xo = bsxfun(@minus,X,m);
X0 = m0-m;
S = S0+Xo*Xo'+kappa0*(X0*X0');

v = (nu-d+1);
r = (1+1/kappa)/v;
p1 = logSt(x,m,r*S,v);

gw0 = GaussWishart(kappa0,m0,nu0,S0);
gw0 = gw0.addData(X);
p0 = gw0.logPredPdf(x);

gw = GaussWishart(kappa0,m0,nu0,S0);
for i=1:n
    gw = gw.addSample(X(:,i));
end
p2 = gw.logPredPdf(x);
maxdiff(p1,p2)
%


% An example file for function 'compute_THD'.
% type 'help compute_THD' for more details.

clc;
close all;

% check cos wave:
'cos wave example'
t = 0:0.0001:1;
freq = 75;
x = 20 * cos(2*pi*freq*t + pi/4);
[ THD_cos, ph, amp ] = compute_THD( t,x, freq );
[THD_cos,  ph*180/pi,  amp]

% check square wave:
'square wave example'
freq = 10; % Hz
number_of_cycles = 4;
dt = 0.0001;
t = 0:dt:(number_of_cycles/freq);
x = (mod(t,1/freq) < 0.5/freq);
x = 1*(2*x-1);
[ THD_square, ph, amp ] = compute_THD( t,x, freq );

plot(t,x); xlabel('t [sec]'); title('square wave'); ylim([-1.1 1.1]);
[THD_square,  ph*180/pi,  amp]

%%% double check THD of square wave using the fourier series:
n = 1:10000;
freq_vec = freq*n;  % fourier series frequency vector
amp_vec = (4/pi)  * (1./n).*(mod(n,2)==1);
% compute THD by definition:
THD_square_theoretical = (sum(amp_vec(2:end).^2) / amp_vec(1)^2)^0.5;
THD_square_theoretical


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve the following exercises:
%   A) Exercise A: Use the Runge-Kutta 4 algorithm to integrate the
%   differential equation:
%            dy/dt = 2*t
%      Integrate from t0=0, to tfinal = 10 s.
%      Compare the solution with the algebraic integration.
%   B) Exercise B: Use the Runge-Kutta 4 algorithm to integrate the
%   second order differential equation:
%            d2y/dt^2+5*dy/dt + 4*y(t) = 0
%
%   C) Exercise C: Use the Runge-Kutta 4 algorithm to simulate the movement
%   of a 1 dof robot arm with friction under the effect of gravity and with
%   zero torque applied.
%
%   D) Exercise D: Use the Runge-Kutta 4 algorithm to simulate the movement
%   of a 2 dof robot arm with friction under the effect of gravity and with
%   zero torques applied.
%
% Help: Function prototype
% [y, t] = runge_kutta(f, y0, [t0 tfinal], timestep)
% where f is the function being integrated as dy/dt = f(t, y).
% y0 are the initial conditions
% t0: initial time
% tfinal: final time.
% h: time step for the calculations.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function runge_kutta_exercises()
close all;

%uncomment to execute each of the exercises
% exerciseA()%
exerciseB()
%exerciseC()
%exerciseD()

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Integrate a simple time function. dy/dt = 2*ts
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exerciseA()
t0 = 0;
tfinal = 10;
[t, y] = runge_kutta(@line, 0, [t0 tfinal], 1);

%Compare with the integral of 2*t
error = y(:)-t(:).^2;
mean(error)
plot(t, y)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use Runge-Kutta to integrate a second order equation of the form.
% d2y/dt^2+5*dy/dt + 4*y(t) = 0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exerciseB()
t0 = 0;
tfinal = 10;
%                                          initial values
[t, y] = runge_kutta(@second_order_system, [10 1]', [t0 tfinal], 0.01);

y = y(:, 1:length(t));
%plot results
plot(t, y(1,:)), hold
plot(t, y(2,:)), hold

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now use Runge-kutta to integrate the movement of a 1 dof robot arm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exerciseC()
%these variables are shared by the forward_dynamic_robot1 function defined
%below
global robot tau g
t0 = 0;
tfinal = 10;
robot = load_robot('example','1dofplanar')
robot.dynamics.friction=0
tau = [0];
g = [0 -9.81 0]';
[t, y] = runge_kutta(@forward_dynamic_robot1, [0 0]', [t0 tfinal], 0.01);

figure, plot(y')
% Animate the movement. Change speed from 1-30-100-200
speed = 5
animate(robot,[y(1,1:speed:length(y)); y(2,1:speed:length(y))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now use Runge-Kutta to simulate the movement of a 2 DOF robot arm.
function exerciseD()
global robot tau g
t0 = 0;
tfinal = 10;
tau = [0 0];
g = [0 -9.81 0]';
robot = load_robot('example','2dofplanar')
robot.dynamics.friction=1

[t, y] = runge_kutta(@forward_dynamic_robot2, [0 0 0 0]', [t0 tfinal], 0.01);

%speed, change to 10, 30, 50, 100
speed = 5
animate(robot,[y(1,1:speed:length(y)); y(2,1:speed:length(y))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function.
% The function returns dy/dt = 2*t. Function called from exerciseA()
%
% Integrate a line in time. dy/dt = 2*t. Obviously, the integration should
% yield. y(t) = t^2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function dy = line(t, y)
dy = 2*t;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to solve a second order differential equation.
% Called from function exerciseB()
%
% In order to solve for d2y/dt^2 + 5*dy/dt + 4*y(t) = 0. We can use:
% x1 = y
% x2 = dy/dt
% thus,
% dx1/dt = x2
% dx2/dt = d2y/dt^2=-5*dy/dt-4*y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = second_order_system(t, y)
% We must return the solution of
% [dx1/dt; dx2/dt]
xd(1) = y(2);
xd(2) = 0 - 5*y(2)-4*y(1);
xd = xd(:);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to simulate the movement of a 1 DOF robot
% Called from function exerciseC()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = forward_dynamic_robot1(t, y)
global tau g robot

qdd = accel(robot, y(1), y(2), tau, g);
%return qd, qdd
xd = [y(2); qdd];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to simulate the movement of a 2 DOF robot
% Called from function exerciseD()
% t is only used to plot time on screen during simulation.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = forward_dynamic_robot2(t, y)
global tau g robot
%we must return the solution of
% [dx1/dt; dx2/dt]
t
% caution, g in the forwarddynamics_2dofplanar takes the opposite sign
qdd=forwarddynamics_2dofplanar(robot, y(1:2,1), y(3:4,1), tau', -sum(g), [0 0 0 0 0 0]);
xd = [y(3:4,1); qdd];


function [phi_g, ip, x_g,  w_g] = boundary(i_bnd, x_local, n_gauss)

% Compute values of 2D quadratic bases at 1D Gauss points along a boundary
% Also return a list of (3) indices for the non-zero basis functions,
% the co-ords of the Gauss points and the 1D integration weights
%
% i_bnd specifies the triangle boundary of interest
%       1 - along s   = 0 boundary   - points [1 4 2]
%       2 - along r+s = 1 boundary   - points [2 5 3]
%       3 - along r   = 0 boundary   - points [3 6 1]
% x_local 6 x 2 array of nodal coordinate points (x, y)
% n_gauss  the number of Gauss points for the 1D integration

% phi_g is a n_gauss array x 3 - values of the 3 non-zero quadratic basis
%                                functions at the n_gauss 1D integration points
% ip  array of indices for non-zero quadratic functions
% x_g   is an n_gauss x 2 array of coordinates for the 1D Gauss points
% w_g   is an n_gauss array of integration weights

%% extract the Gauss weights and points on [- 1, 1];
     [r,  w_g] = oned_gauss(n_gauss); % 1 \le n_gauss \le 6

     r1 = (1+r)/2; % map to [0, 1]
     r1 = r1(:);   % make sure it's a column

%% which boundary is requested
     switch i_bnd
         case {1}
             ip = [ 1 4 2];
         case {2}
             ip = [ 2 5 3];
         case {3}
             ip = [ 3 6 1];
         otherwise
             disp(['Illegal i_bnd = ', int2str(i_bnd)])
             return
     end

%% Coordinates of the Gauss points
     x_g = (1-r1)*x_local(ip(1),:) ...
          +   r1 *x_local(ip(3),:);

%% Scale the weights by the (unsigned) length of the interval
     djac  =  norm(x_local(ip(3), :) - x_local(ip(1), :))/2;
     w_g   =  w_g*djac;

%% Evaluate the test functions

      p1 = 1 -3*r1 + 2*r1.^2; % p1(r = 0)  = 1
      p2 =    4*r1 - 4*r1.^2; % p2(r = 1/2)= 1
      p3 =   -  r1 + 2*r1.^2; % p3(r = 1)  = 1

      phi_g = [p1(:)  p2(:) p3(:) ];

  return
end