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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

%%%% TODO %%%%

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    %%%% reachableForward
    %%%% reachableForwardRel
end


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

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

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

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

  return
end


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

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

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

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

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

  return
end


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

%	P. Goncalves, October 1995
%	Copyright (c) 1995 Rice University
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

clf;
tfr = abs(tfr) ;

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

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


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

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

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

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

    opt = tb_optparse(opt, varargin);

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

	w = [];
    MX = [];

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

    if nargout > 1
        mx = MX;
    end

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

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

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

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

    if nargout > 1
        mx = Mx;
    end


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

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

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

  return
end


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

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

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

    i=1;
    j=1;

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

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

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

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

        i=i+1;
    end

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

return


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

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

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

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

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

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

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

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

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

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


function x = nco_abscissas_ab ( a, b, n )

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

  return
end


function mono_between_enum_test ( )

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

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

  return
end


function [h] = fdr(p, q)

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

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

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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

    for j = k : r

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

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

    end

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    end

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

  return
end


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

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

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


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

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


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

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

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

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

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


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

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

  x = mu + sigma * y;

  return
end


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

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


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

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

cont=0;
s=0;

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

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


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

% Copyright 2011 The MathWorks, Inc.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

end


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

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

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

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

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

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

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

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

% compute centroid
m = mean(x);

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


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

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

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

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

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

% handle incorrectly given optional args

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

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

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

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

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

g = h1.*h2;

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


function [ errors ] = test_operators( )

errors = 0;

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

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

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

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

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

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

errors = errors + test_kron_reduction();

end

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

end

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

end

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

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

end

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

x = rand(N,1);

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

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

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

end

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

x = rand(N-1,1);

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

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

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

end

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

M = 5;

x = rand(N,M);

t = gsp_grad(G,x);

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

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

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

end

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

M = 5;

x = rand(N,M);

t = gsp_grad(G,x);

y1 = gsp_div(G,t);

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

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

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

end

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

x = rand(N,1);

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

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

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

end

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

M = 5;

x = rand(N,M);

y1 = gsp_grad(G,x);

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

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

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

end

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

x = rand(N,1);

y1 = gsp_grad(G,x);

D = gsp_grad_mat(G);

y2 = D*x;

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

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

end

function errors = test_directed()
errors = 0;

try

    G = gsp_sensor(50);

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

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

end

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

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

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

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

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

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

end

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

x = rand(N,1);

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

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

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

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

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

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

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

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

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

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

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

end

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

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

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

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

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

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

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

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

end

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

f = rand(N,1);

ft = gsp_translate(G,f,k);

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

end

end

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

f = rand(N,1);

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

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

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

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

end

end

function errors = test_kron_reduction()

errors = 0;

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

    close(100)

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

end

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

end

function errors = test_gft()

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

X = rand(N,20,10);

Xhat1 = gsp_gft(G,X);

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

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

end


function y = load_signal(name, n, options)

% load_signal - load a 1D signal
%
%   y = load_signal(name, n, options);
%
% name is a string that can be :
%   'regular' (options.alpha gives regularity)
%   'step', 'rand',
%   'gaussiannoise' (options.sigma gives width of filtering in pixels),
%   [natural signals]
%   'tiger', 'bell', 'bird'
%   [WAVELAB signals]
%   'HeaviSine', 'Bumps', 'Blocks',
%   'Doppler', 'Ramp', 'Cusp', 'Sing', 'HiSine',
%   'LoSine', 'LinChirp', 'TwoChirp', 'QuadChirp',
%   'MishMash', 'WernerSorrows' (Heisenberg),
%   'Leopold' (Kronecker), 'Piece-Regular' (Piece-Wise Smooth),
%	'Riemann','HypChirps','LinChirps', 'Chirps', 'Gabor'
%	'sineoneoverx','Cusp2','SmoothCusp','Gaussian'
%	'Piece-Polynomial' (Piece-Wise 3rd degree polynomial)

if nargin<2
    n = 1024;
end
options.null = 0;
if isfield(options, 'alpha')
    alpha = options.alpha;
else
    alpha = 2;
end

options.rep = '';
switch lower(name)
    case 'regular'
        y = gen_signal(n,alpha);
    case 'step'
        y = linspace(0,1,n)>0.5;
    case 'stepregular'
        y = linspace(0,1,n)>0.5; y=y(:);
        a = gen_signal(n,2); a = a(:);
        a = rescale(a,-0.1,0.1);
        y = y+a;
    case 'gaussiannoise'
        % filtered gaussian noise
        y = randn(n,1);
        if isfield(options, 'sigma')
            sigma = options.sigma; % variance in number of pixels
        else
            sigma = 20;
        end
        m = min(n, 6*round(sigma/2)+1);
        h = compute_gaussian_filter(m,sigma/(4*n),n);
        options.bound = 'per';
        y = perform_convolution(y,h, options);
    case 'rand'
        if isfield(options, 'p1')
            p1 = options.p1;
        else
            c = 10;
            p1 = 1:c; p1 = p1/sum(p1);
        end
        p1 = p1(:); c = length(p1);
        if isfield(options, 'p2')
            p2 = options.p2;
        else
            if isfield(options, 'evol')
                evol = options.evol;
            else
                evol = 0;
            end
            p2 = p1(:) + evol*(rand(c,1)-0.5);
            p2 = max(p2,0); p2 = p2/sum(p2);
        end
        y = zeros(n,1);
        for i=1:n
            a = (i-1)/(n-1);
            p = a*p1+(1-a)*p2; p = p/sum(p);
            y(i) = rand_discr(p, 1);
        end
    case 'bird'
        [y,fs] = load_sound([name '.wav'], n, options);
    case 'tiger'
        [y,fs] = load_sound([name '.au'], n, options);
    case 'bell'
        [y,fs] = load_sound([name '.wav'], n, options);
    otherwise
        y = MakeSignal(name,n);
end

y = y(:);

function y = gen_signal(n,alpha)

%   gen_signal -  generate a 1D C^\alpha signal of length n.
%
%   y = gen_signal(n,alpha);
%
%   The signal is scaled in [0,1].
%
%   Copyright (c) 2003 Gabriel Peyr?

if nargin<2
    alpha = 2;
end

y = randn(n,1);
fy = fft(y);
fy = fftshift(fy);
% filter with |omega|^{-\alpha}
h = (-n/2+1):(n/2);
h = (abs(h)+1).^(-alpha-0.5);
fy = fy.*h';
fy = fftshift(fy);
y = real( ifft(fy) );

y = (y-min(y))/(max(y)-min(y));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function sig = MakeSignal(Name,n)
% MakeSignal -- Make artificial signal
%  Usage
%    sig = MakeSignal(Name,n)
%  Inputs
%    Name   string: 'HeaviSine', 'Bumps', 'Blocks',
%            'Doppler', 'Ramp', 'Cusp', 'Sing', 'HiSine',
%            'LoSine', 'LinChirp', 'TwoChirp', 'QuadChirp',
%            'MishMash', 'WernerSorrows' (Heisenberg),
%            'Leopold' (Kronecker), 'Piece-Regular' (Piece-Wise Smooth),
%	     'Riemann','HypChirps','LinChirps', 'Chirps', 'Gabor'
%	     'sineoneoverx','Cusp2','SmoothCusp','Gaussian'
%	     'Piece-Polynomial' (Piece-Wise 3rd degree polynomial)
%    n      desired signal length
%  Outputs
%    sig    1-d signal
%
%  References
%    Various articles of D.L. Donoho and I.M. Johnstone
%
if nargin > 1,
    t = (1:n) ./n;
end
Name = lower(Name);
if strcmp(Name,'heavisine'),
    sig = 4.*sin(4*pi.*t);
    sig = sig - sign(t - .3) - sign(.72 - t);
elseif strcmp(Name,'bumps'),
    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
    hgt = [ 4  5   3   4  5  4.2 2.1 4.3  3.1 5.1 4.2];
    wth = [.005 .005 .006 .01 .01 .03 .01 .01  .005 .008 .005];
    sig = zeros(size(t));
    for j =1:length(pos)
        sig = sig + hgt(j)./( 1 + abs((t - pos(j))./wth(j))).^4;
    end
elseif strcmp(Name,'blocks'),
    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
    hgt = [4 (-5) 3 (-4) 5 (-4.2) 2.1 4.3  (-3.1) 2.1 (-4.2)];
    sig = zeros(size(t));
    for j=1:length(pos)
        sig = sig + (1 + sign(t-pos(j))).*(hgt(j)/2) ;
    end
elseif strcmp(Name,'doppler'),
    sig = sqrt(t.*(1-t)).*sin((2*pi*1.05) ./(t+.05));
elseif strcmp(Name,'ramp'),
    sig = t - (t >= .37);
elseif strcmp(Name,'cusp'),
    sig = sqrt(abs(t - .37));
elseif strcmp(Name,'sing'),
    k = floor(n * .37);
    sig = 1 ./abs(t - (k+.5)/n);
elseif strcmp(Name,'hisine'),
    sig = sin( pi * (n * .6902) .* t);
elseif strcmp(Name,'losine'),
    sig = sin( pi * (n * .3333) .* t);
elseif strcmp(Name,'linchirp'),
    sig = sin(pi .* t .* ((n .* .500) .* t));
elseif strcmp(Name,'twochirp'),
    sig = sin(pi .* t .* (n .* t)) + sin((pi/3) .* t .* (n .* t));
elseif strcmp(Name,'quadchirp'),
    sig = sin( (pi/3) .* t .* (n .* t.^2));
elseif strcmp(Name,'mishmash'),  % QuadChirp + LinChirp + HiSine
    sig = sin( (pi/3) .* t .* (n .* t.^2)) ;
    sig = sig +  sin( pi * (n * .6902) .* t);
    sig = sig +  sin(pi .* t .* (n .* .125 .* t));
elseif strcmp(Name,'wernersorrows'),
    sig = sin( pi .* t .* (n/2 .* t.^2)) ;
    sig = sig +  sin( pi * (n * .6902) .* t);
    sig = sig +  sin(pi .* t .* (n .* t));
    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
    hgt = [ 4  5   3   4  5  4.2 2.1 4.3  3.1 5.1 4.2];
    wth = [.005 .005 .006 .01 .01 .03 .01 .01  .005 .008 .005];
    for j =1:length(pos)
        sig = sig + hgt(j)./( 1 + abs((t - pos(j))./wth(j))).^4;
    end
elseif strcmp(Name,'leopold'),
    sig = (t == floor(.37 * n)/n);  % Kronecker
elseif strcmp(Name,'riemann'),
    sqn = round(sqrt(n));
    sig = t .* 0;  % Riemann's Non-differentiable Function
    sig((1:sqn).^2) = 1. ./ (1:sqn);
    sig = real(ifft(sig));
elseif strcmp(Name,'hypchirps'), % Hyperbolic Chirps of Mallat's book
    alpha	= 15*n*pi/1024;
    beta    = 5*n*pi/1024;
    t  	= (1.001:1:n+.001)./n;
    f1      = zeros(1,n);
    f2      = zeros(1,n);
    f1  	= sin(alpha./(.8-t)).*(0.1<t).*(t<0.68);
    f2  	= sin(beta./(.8-t)).*(0.1<t).*(t<0.75);
    M  	= round(0.65*n);
    P 	= floor(M/4);
    enveloppe = ones(1,M); % the rising cutoff function
    enveloppe(1:P) = (1+sin(-pi/2+((1:P)-ones(1,P))./(P-1)*pi))/2;
    enveloppe(M-P+1:M) = reverse(enveloppe(1:P));
    env 	= zeros(1,n);
    env(ceil(n/10):M+ceil(n/10)-1) = enveloppe(1:M);
    sig     = (f1+f2).*env;
elseif strcmp(Name,'linchirps'), % Linear Chirps of Mallat's book
    b 	= 100*n*pi/1024;
    a 	= 250*n*pi/1024;
    t 	= (1:n)./n;
    A1 	= sqrt((t-1/n).*(1-t));
    sig	= A1.*(cos((a*(t).^2)) + cos((b*t+a*(t).^2)));
elseif strcmp(Name,'chirps'), % Mixture of Chirps of Mallat's book
    t 	= (1:n)./n.*10.*pi;
    f1 	= cos(t.^2*n/1024);
    a 	= 30*n/1024;
    t 	= (1:n)./n.*pi;
    f2 	= cos(a.*(t.^3));
    f2 	= reverse(f2);
    ix 	= (-n:n)./n.*20;
    g 	= exp(-ix.^2*4*n/1024);
    i1 	= (n/2+1:n/2+n);
    i2 	= (n/8+1:n/8+n);
    j  	= (1:n)/n;
    f3 	= g(i1).*cos(50.*pi.*j*n/1024);
    f4 	= g(i2).*cos(350.*pi.*j*n/1024);
    sig 	= f1+f2+f3+f4;
    enveloppe = ones(1,n); % the rising cutoff function
    enveloppe(1:n/8) = (1+sin(-pi/2+((1:n/8)-ones(1,n/8))./(n/8-1)*pi))/2;
    enveloppe(7*n/8+1:n) = reverse(enveloppe(1:n/8));
    sig 	= sig.*enveloppe;
elseif strcmp(Name,'gabor'), % two modulated Gabor functions in
    % Mallat's book
    N = 512;
    t = (-N:N)*5/N;
    j = (1:N)./N;
    g = exp(-t.^2*20);
    i1 = (2*N/4+1:2*N/4+N);
    i2 = (N/4+1:N/4+N);
    sig1 = 3*g(i1).*exp(i*N/16.*pi.*j);
    sig2 = 3*g(i2).*exp(i*N/4.*pi.*j);
    sig = sig1+sig2;
elseif strcmp(Name,'sineoneoverx'), % sin(1/x) in Mallat's book
    N = 1024;
    a = (-N+1:N);
    a(N) = 1/100;
    a = a./(N-1);
    sig = sin(1.5./(i));
    sig = sig(513:1536);
elseif strcmp(Name,'cusp2'),
    N = 64;
    a = (1:N)./N;
    x = (1-sqrt(a)) + a/2 -.5;
    M = 8*N;
    sig = zeros(1,M);
    sig(M-1.5.*N+1:M-.5*N) = x;
    sig(M-2.5*N+2:M-1.5.*N+1) = reverse(x);
    sig(3*N+1:3*N + N) = .5*ones(1,N);
elseif strcmp(Name,'smoothcusp'),
    sig = MakeSignal('Cusp2');
    N = 64;
    M = 8*N;
    t = (1:M)/M;
    sigma = 0.01;
    g = exp(-.5.*(abs(t-.5)./sigma).^2)./sigma./sqrt(2*pi);
    g = fftshift(g);
    sig2 = iconv(g',sig)'/M;
elseif strcmp(Name,'piece-regular'),
    sig1=-15*MakeSignal('Bumps',n);
    t = (1:fix(n/12)) ./fix(n/12);
    sig2=-exp(4*t);
    t = (1:fix(n/7)) ./fix(n/7);
    sig5=exp(4*t)-exp(4);
    t = (1:fix(n/3)) ./fix(n/3);
    sigma=6/40;
    sig6=-70*exp(-((t-1/2).*(t-1/2))/(2*sigma^2));
    sig(1:fix(n/7))= sig6(1:fix(n/7));
    sig((fix(n/7)+1):fix(n/5))=0.5*sig6((fix(n/7)+1):fix(n/5));
    sig((fix(n/5)+1):fix(n/3))=sig6((fix(n/5)+1):fix(n/3));
    sig((fix(n/3)+1):fix(n/2))=sig1((fix(n/3)+1):fix(n/2));
    sig((fix(n/2)+1):(fix(n/2)+fix(n/12)))=sig2;
    sig((fix(n/2)+2*fix(n/12)):-1:(fix(n/2)+fix(n/12)+1))=sig2;
    sig(fix(n/2)+2*fix(n/12)+fix(n/20)+1:(fix(n/2)+2*fix(n/12)+3*fix(n/20)))=...
        -ones(1,fix(n/2)+2*fix(n/12)+3*fix(n/20)-fix(n/2)-2*fix(n/12)-fix(n/20))*25;
    k=fix(n/2)+2*fix(n/12)+3*fix(n/20);
    sig((k+1):(k+fix(n/7)))=sig5;
    diff=n-5*fix(n/5);
    sig(5*fix(n/5)+1:n)=sig(diff:-1:1);
    % zero-mean
    bias=sum(sig)/n;
    sig=bias-sig;
elseif strcmp(Name,'piece-polynomial'),
    t = (1:fix(n/5)) ./fix(n/5);
    sig1=20*(t.^3+t.^2+4);
    sig3=40*(2.*t.^3+t) + 100;
    sig2=10.*t.^3 + 45;
    sig4=16*t.^2+8.*t+16;
    sig5=20*(t+4);
    sig6(1:fix(n/10))=ones(1,fix(n/10));
    sig6=sig6*20;
    sig(1:fix(n/5))=sig1;
    sig(2*fix(n/5):-1:(fix(n/5)+1))=sig2;
    sig((2*fix(n/5)+1):3*fix(n/5))=sig3;
    sig((3*fix(n/5)+1):4*fix(n/5))=sig4;
    sig((4*fix(n/5)+1):5*fix(n/5))=sig5(fix(n/5):-1:1);
    diff=n-5*fix(n/5);
    sig(5*fix(n/5)+1:n)=sig(diff:-1:1);
    %sig((fix(n/20)+1):(fix(n/20)+fix(n/10)))=-ones(1,fix(n/10))*20;
    sig((fix(n/20)+1):(fix(n/20)+fix(n/10)))=ones(1,fix(n/10))*10;
    sig((n-fix(n/10)+1):(n+fix(n/20)-fix(n/10)))=ones(1,fix(n/20))*150;
    % zero-mean
    bias=sum(sig)/n;
    sig=sig-bias;
elseif strcmp(Name,'gaussian'),
    sig=GWN(n,beta);
    g=zeros(1,n);
    lim=alpha*n;
    mult=pi/(2*alpha*n);
    g(1:lim)=(cos(mult*(1:lim))).^2;
    g((n/2+1):n)=g((n/2):-1:1);
    g = rnshift(g,n/2);
    g=g/norm(g);
    sig=iconv(g,sig);
else
    disp(sprintf('MakeSignal: I don*t recognize <<%s>>',Name))
    disp('Allowable Names are:')
    disp('HeaviSine'),
    disp('Bumps'),
    disp('Blocks'),
    disp('Doppler'),
    disp('Ramp'),
    disp('Cusp'),
    disp('Crease'),
    disp('Sing'),
    disp('HiSine'),
    disp('LoSine'),
    disp('LinChirp'),
    disp('TwoChirp'),
    disp('QuadChirp'),
    disp('MishMash'),
    disp('WernerSorrows'),
    disp('Leopold'),
    disp('Sing'),
    disp('HiSine'),
    disp('LoSine'),
    disp('LinChirp'),
    disp('TwoChirp'),
    disp('QuadChirp'),
    disp('MishMash'),
    disp('WernerSorrows'),
    disp('Leopold'),
    disp('Riemann'),
    disp('HypChirps'),
    disp('LinChirps'),
    disp('Chirps'),
    disp('sineoneoverx'),
    disp('Cusp2'),
    disp('SmoothCusp'),
    disp('Gabor'),
    disp('Piece-Regular');
    disp('Piece-Polynomial');
    disp('Gaussian');
end

%
% Originally made by David L. Donoho.
% Function has been enhanced.

%
% Part of WaveLab Version 802
% Built Sunday, October 3, 1999 8:52:27 AM
% This is Copyrighted Material
% For Copying permissions see COPYING.m
% Comments? e-mail wavelab@stat.stanford.edu
%


function line_fekete_rule_test02 ( m )

%*****************************************************************************80
%
%% LINE_FEKETE_RULE_TEST02 seeks Fekete points in [-1,+1].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 March 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    L Bos, N Levenberg,
%    On the calculation of approximate Fekete points: the univariate case,
%    Electronic Transactions on Numerical Analysis,
%    Volume 30, pages 377-397, 2008.
%
%  Parameters:
%
%    Input, integer M, the dimension of the polynomial space.
%
  if ( nargin < 1 )
    m = 5;
  end
  n = 1000;
  a = -1.0;
  b = +1.0;
  x = linspace ( a, b, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LINE_FEKETE_RULE_TEST02:\n' );
  fprintf ( 1, '  Seek Fekete points in [%g,%g]\n', a, b );
  fprintf ( 1, '  using %d equally spaced sample points\n', n );
  fprintf ( 1, '  for polynomials of degree M = %d\n', m );
  fprintf ( 1, '  with the Chebyshev basis\n' );
  fprintf ( 1, '  and weight 1/sqrt(1-x^2).\n' );

  [ nf, xf, wf, vf ] = line_fekete_chebyshev ( m, a, b, n, x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  NF = %d\n', nf );
  r8vec_print ( nf, xf, '  Estimated Fekete points XF:' );

  yf = ones ( size ( xf ) );
  plot ( xf, yf, '*' );
  title ( 'Estimated Fekete points' );
  grid on
  filename = 'line_fekete_rule_test02.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plotfile as "%s"\n', filename );

  wf_sum = sum ( wf(1:nf) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sum(WF) = %g\n', wf_sum );

  return
end


%% Unimodal ODF Shapes
%
%%
% Also take a look at the page <SO3Kernels.html SO3Kernels>.
%
% In order to control the shape of unimodal ODF
% The classes @SO3Kernel and @S2Kernel are needed in MTEX to define the
% specific form of unimodal and fibre symmetric ODFs. It has to be passed
% as an argument when calling the methods <uniformODF.html uniformODF> or
% <fibreODF.html fibreODF>.
%
% A kernel is defined by specifying its name and its free parameter.
% Alternatively one can also specify the halfwidth of the kernel. Below you
% find a list of some important SO3Kernel functions supported by MTEX.

psi{1} = SO3AbelPoissonKernel(0.79);
psi{2} = SO3DeLaValleePoussinKernel(13);
psi{3} = SO3BumpKernel(35*degree);
psi{4} = SO3DirichletKernel(3);
psi{5} = SO3vonMisesFisherKernel(7.5);
psi{6} = SO3GaussWeierstrassKernel(0.07);
psi{7} = fibreVonMisesFisherKernel(7.2);
psi{8} = SO3SquareSingularityKernel(0.72);

%%
% Lets visualize these kernel functions as one dimensional sections through
% the orientation space

% the kernel on SO(3)
close;
figure('position',[100,100,1000,450])
hold all
for i = 1:numel(psi)
  plot(psi{i},'DisplayName',class(psi{i}));
end
hold off
legend(gca,'show','Location','eastoutside')

%%
% one dimensional sections through the corresponding PDF

close; figure('position',[100,100,1000,450])
hold all
for i = 1:numel(psi)
  plot(psi{i}.radon,'symmetric','DisplayName',class(psi{i}),'linewidth',2);
end
hold off

ylim([-5,20])

legend(gca,'show','Location','eastoutside')
%%
% the Fourier coefficients of the kernels

close; figure('position',[100,100,500,450])
hold all
for i = 1:numel(psi)
  plotSpektra(psi{i},'bandwidth',32,'linewidth',2,'DisplayName',class(psi{i}));
end
hold off
legend(gca,'show')


function value = r4_binom ( n, m )

%*****************************************************************************80
%
%% R4_BINOM evaluates the binomial coefficient using R4 arithmetic.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, integer N, M, the arguments.
%
%    Output, real VALUE, the binomial coefficient.
%
  persistent sq2pil
  persistent bilnmx
  persistent fintmx

  sq2pil = 0.91893853320467274;

  if ( isempty ( bilnmx ) )
    bilnmx = log ( r4_mach ( 2 ) );
    fintmx = 0.9 / r4_mach ( 3 );
  end

  if ( n < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_BINOM - Fatal error!\n' );
    fprintf ( 1, '  N < 0.\n' );
    error ( 'R4_BINOM - Fatal error!' )
  end

  if ( m < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_BINOM - Fatal error!\n' );
    fprintf ( 1, '  M < 0.\n' );
    error ( 'R4_BINOM - Fatal error!' )
  end

  if ( n < m )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_BINOM - Fatal error!\n' );
    fprintf ( 1, '  N < M.\n' );
    error ( 'R4_BINOM - Fatal error!' )
  end

  k = min ( m, n - m );

  if ( k <= 20 && k * log ( max ( n, 1 ) ) <= bilnmx )

    value = 1.0;

    for i = 1 : k
      value = value * ( n - i + 1 ) / i;
    end

  else

    if ( k < 9 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R4_BINOM - Fatal error!\n' );
      fprintf ( 1, '  Result overflows.\n' );
      fprintf ( 1, '  N or M is too big.\n' );
      error ( 'R4_BINOM - Fatal error!' )
    end

    xn = n + 1;
    xk = k + 1;
    xnk = n - k + 1;

    corr = r4_lgmc ( xn ) - r4_lgmc ( xk ) - r4_lgmc ( xnk );

    value = xk * log ( xnk / xk ) ...
      - xn * r4_lnrel ( - ( xk - 1.0 ) / xn ) ...
      - 0.5 * log ( xn * xnk / xk ) + 1.0 - sq2pil + corr;

    if ( bilnmx < value )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R4_BINOM - Fatal error!\n' );
      fprintf ( 1, '  Result overflows.\n' );
      fprintf ( 1, '  N or M is too big.\n' );
      error ( 'R4_BINOM - Fatal error!' )
    end

    value = exp ( value );

  end

  if ( value < fintmx )
    value = r4_aint ( value + 0.5 );
  end

  return
end


function x = spm_invGcdf(F,h,l,tol)
% Inverse Cumulative Distribution Function (CDF) of Gamma distribution
% FORMAT x = spm_invGcdf(p,h,l,tol)
%
% F   - CDF (lower tail p-value)
% h   - Shape parameter (h>0)
% l   - Scale parameter (l>0)
% x   - Gamma ordinates at which CDF F(x)=F
% tol - tolerance [default 10^-6]
%__________________________________________________________________________
%
% spm_invGcdf implements the inverse Cumulative Distribution Function
% for Gamma distributions.
%
% Definition:
%--------------------------------------------------------------------------
% The Gamma distribution has shape parameter h and scale l, and is
% defined for h>0 & l>0 and for x in [0,Inf) (See Evans et al., Ch18,
% but note that this reference uses the alternative parameterisation of
% the Gamma with scale parameter c=1/l)
%
% The Cumulative Distribution Function (CDF) F(x) is the probability
% that a realisation of a Gamma random variable X has value less than
% x. F(x)=Pr{X<x}:  (See Evans et al., Ch18)
%
% Variate relationships: (Evans et al., Ch8 & Ch18)
%--------------------------------------------------------------------------
% For natural (strictly +ve integer) shape h this is an Erlang distribution.
%
% The Standard Gamma distribution has a single parameter, the shape h.
% The scale taken as l=1.
%
% The Chi-squared distribution with v degrees of freedom is equivalent
% to the Gamma distribution with scale parameter 1/2 and shape parameter v/2.
%
% Algorithm:
%--------------------------------------------------------------------------
% Interval bisection to find the inverse CDF to within tol.
%
% References:
%--------------------------------------------------------------------------
% Evans M, Hastings N, Peacock B (1993)
%       "Statistical Distributions"
%        2nd Ed. Wiley, New York
%
% Abramowitz M, Stegun IA, (1964)
%       "Handbook of Mathematical Functions"
%        US Government Printing Office
%
% Press WH, Teukolsky SA, Vetterling AT, Flannery BP (1992)
%       "Numerical Recipes in C"
%        Cambridge
%__________________________________________________________________________
% Copyright (C) 1992-2011 Wellcome Trust Centre for Neuroimaging

% Andrew Holmes
% $Id: spm_invGcdf.m 4182 2011-02-01 12:29:09Z guillaume $

%-Parameters
%--------------------------------------------------------------------------
Dtol   = 10^-8;
maxIt = 10000;

%-Format arguments, note & check sizes
%--------------------------------------------------------------------------
if nargin<4, tol = Dtol; end
if nargin<3, error('Insufficient arguments'), end

ad = [ndims(F);ndims(h);ndims(l)];
rd = max(ad);
as = [[size(F),ones(1,rd-ad(1))];...
      [size(h),ones(1,rd-ad(2))];...
      [size(l),ones(1,rd-ad(3))]];
rs = max(as);
xa = prod(as,2)>1;
if sum(xa)>1 && any(any(diff(as(xa,:)),1))
    error('non-scalar args must match in size');
end

%-Computation - Undefined and special cases
%--------------------------------------------------------------------------
%-Initialise result to zeros
x = zeros(rs);

%-Only defined for F in [0,1] & strictly positive h & l.
% Return NaN if undefined.
md = ( F>=0  &  F<=1  &  h>0  &  l>0 );
if any(~md(:))
    x(~md) = NaN;
    warning('Returning NaN for out of range arguments');
end

%-Special cases: x=0 when F=0, x=Inf when F=1
x(md & F==1) = Inf;

%-Compute where defined & not special case
%--------------------------------------------------------------------------
Q  = find( md  &  F>0  &  F<1 );
if isempty(Q), return, end
if xa(1), FQ=F(Q); FQ=FQ(:); else FQ=F*ones(length(Q),1); end
if xa(2), hQ=h(Q); hQ=hQ(:); else hQ=h*ones(length(Q),1); end
if xa(3), lQ=l(Q); lQ=lQ(:); else lQ=l*ones(length(Q),1); end
%-?Q=?Q(:) stuff is to avoid discrepant orientations of vector arguments!

%-Compute initial interval estimates [0,mean] & Grow to encompass F(QF);
%--------------------------------------------------------------------------
a  = zeros(length(Q),1);
b  = hQ./(2*lQ);
QQ = 1:length(Q);
while ~isempty(QQ)
    b(QQ) = 2*b(QQ);
    QQ    = find(spm_Gcdf(b,hQ,lQ) < FQ);
end

%-Interval bisection
%--------------------------------------------------------------------------
fa = spm_Gcdf(a,hQ,lQ)-FQ;
fb = spm_Gcdf(b,hQ,lQ)-FQ;
i  = 0;
xQ = a+(b-a)/2;
QQ = 1:length(Q);

while ~isempty(QQ) &&  i<maxIt
    fxQQ        = spm_Gcdf(xQ(QQ),hQ(QQ),lQ(QQ))-FQ(QQ);
    mQQ         = fa(QQ).*fxQQ > 0;
    a(QQ(mQQ))  = xQ(QQ(mQQ));   fa(QQ(mQQ))  = fxQQ(mQQ);
    b(QQ(~mQQ)) = xQ(QQ(~mQQ));  fb(QQ(~mQQ)) = fxQQ(~mQQ);
    xQ(QQ)      = a(QQ) + (b(QQ)-a(QQ))/2;
    QQ          = QQ( (b(QQ)-a(QQ))>tol );
    i           = i+1;
end

if i==maxIt
    warning('convergence criteria not reached - maxIt reached');
end

x(Q) = xQ;


function E = quaternion2EulXYZ(Q)
% Euler/Tait-Bryan angles; x-y-z extrinsic convention
%
% Parameters
% ------------
%   Q : 1 x 4 float vector
%       Quaternion
%
% Returns
% ------------
%   E : 1 x 3 float vector
%       Euler angles equivalent of the quaternion (radian)
%       E(1) rotation about x, E(2) rotation about y, E(3) rotation about z
%
E = zeros(3,1);
E(1) = atan2((2*(Q(1)*Q(2)+Q(3)*Q(4))), (1-2*(Q(2)^2+Q(3)^2)));
E(2) = asin(  2*(Q(1)*Q(3)-Q(4)*Q(2)));
E(3) = atan2((2*(Q(1)*Q(4)+Q(2)*Q(3))), (1-2*(Q(3)^2+Q(4)^2)));
end


function  R = MatrixExp3(so3mat)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a 3x3 so(3) representation of exponential coordinates.
% Returns R in SO(3) that is achieved by rotating about omghat by theta
% from an initial orientation R = I.
% Example Input:
%
% clear; clc;
% so3mat = [[0, -3, 2]; [3, 0, -1]; [-2, 1, 0]];
% R = MatrixExp3(so3mat)
%
% Output:
% R =
%   -0.6949    0.7135    0.0893
%   -0.1920   -0.3038    0.9332
%    0.6930    0.6313    0.3481

omgtheta = so3ToVec(so3mat);
if NearZero(norm(omgtheta))
    R = eye(3);
else
    [omghat, theta] = AxisAng3(omgtheta);
    omgmat = so3mat / theta;
    R = eye(3) + sin(theta) * omgmat + (1 - cos(theta)) * omgmat * omgmat;
end
end


function [ux,u,m] = applypca2(X)
% [ux,u,m] = applypca(X)
%
% finds principal components of
%
% input:
%  x  input data (each column is an input vector)
%
% output:
%  ux  data projected onto pca (1st row=leading principal component)
%  u   the orthogonal matrix with all principal components
%  m   mean of the data
%
% You can recover the original data with
%
% xoriginal=u*ux+repmat(m,1,size(ux,2));
%
% See also pca
%
% copyright by Kilian Q. Weinberger, 2006

% [u,v]=pca(X);
[d,N]  = size(X);

m = mean(X,2);
X = X - m*ones(1,N); % remove mean from data

cc = cov(X',1); % compute covariance
[cvv,cdd] = eig(cc); % compute eignvectors
[zz,ii] = sort(diag(-cdd)); % sort according to eigenvalues
u = cvv(:,ii); % pick leading eigenvectors

ux=u'*X;


function g=comp_pchirp(L,n)
%COMP_PCHIRP  Compute periodic chirp
%   Usage:  g=comp_pchirp(L,n);
%
%   `pchirp(L,n)` returns a periodic, discrete chirp of length *L* that
%   revolves *n* times around the time-frequency plane in frequency. *n* must be
%   an integer number.
%
%   This is a computational routine. Do not call it unless you have
%   verified the input parameters.

%   AUTHOR : Peter L. Søndergaard
%   TESTING: OK
%   REFERENCE: OK

l= (0:L-1).';
X = mod(mod(mod(n*l,2*L).*l,2*L)*(L+1),2*L);
g = exp(pi*1i*X/L);


function [A_hat E_hat iter] = exact_alm_rpca(D, lambda, tol, maxIter)

% Oct 2009
% This matlab code implements the augmented Lagrange multiplier method for
% Robust PCA.
%
% D - m x n matrix of observations/data (required input)
%
% lambda - weight on sparse error term in the cost function
%
% tol - tolerance for stopping criterion.
%     - DEFAULT 1e-7 if omitted or -1.
%
% maxIter - maximum number of iterations
%         - DEFAULT 1000, if omitted or -1.
%
% Initialize A,E,Y,u
% while ~converged
%   minimize
%     L(A,E,Y,u) = |A|_* + lambda * |E|_1 + <Y,D-A-E> + mu/2 * |D-A-E|_F^2;
%   Y = Y + \mu * (D - A - E);
%   \mu = \rho * \mu;
% end
%
% Minming Chen, October 2009. Questions? v-minmch@microsoft.com ;
% Arvind Ganesh (abalasu2@illinois.edu)
%
% Copyright: Perception and Decision Laboratory, University of Illinois, Urbana-Champaign
%            Microsoft Research Asia, Beijing

% addpath PROPACK;

[m n] = size(D);

if nargin < 2
    lambda = 1 / sqrt(m);
end

if nargin < 3
    tol = 1e-7;
elseif tol == -1
    tol = 1e-7;
end

if nargin < 4
    maxIter = 1000;
elseif maxIter == -1
    maxIter = 1000;
end

% initialize
Y = sign(D);
%norm_two = lansvd(Y, 1, 'L');
norm_two = svdex(Y, 1, struct('svd_solver','svds'));
norm_inf = norm( Y(:), inf) / lambda;
dual_norm = max(norm_two, norm_inf);
Y = Y / dual_norm;

A_hat = zeros( m, n);
E_hat = zeros( m, n);
dnorm = norm(D, 'fro');
tolProj = 1e-6 * dnorm;
total_svd = 0;
mu = .5/norm_two; % this one can be tuned
rho = 6;          % this one can be tuned

iter = 0;
converged = false;
stopCriterion = 1;
sv = 5;
svp = sv;

% excelly
temp_T = D - A_hat + (1/mu)*Y;
temp_E = max( temp_T - lambda/mu,0) + min( temp_T + lambda/mu,0);
[U dummy dummy] = svdex(D - temp_E + (1/mu)*Y, 1, struct('svd_solver','svds'));
while ~converged
    iter = iter + 1;

    % solve the primal problem by alternative projection
    primal_converged = false;
    primal_iter = 0;
    sv = sv + round(n * 0.1);
    while primal_converged == false

        temp_T = D - A_hat + (1/mu)*Y;
        temp_E = max( temp_T - lambda/mu,0) + min( temp_T + lambda/mu,0);

        [U diagS V] = svdex(D - temp_E + (1/mu)*Y, sv, struct('init',U(:,1)));
        % if choosvd(n, sv) == 1
        %     [U S V] = lansvd(D - temp_E + (1/mu)*Y, sv, 'L');
        % else
        %     [U S V] = svd(D - temp_E + (1/mu)*Y, 'econ');
        % end
        % diagS = diag(S);
        svp = length(find(diagS > 1/mu));
        if svp < sv
            sv = min(svp + 1, n);
        else
            sv = min(svp + round(0.05*n), n);
        end
        temp_A = U(:,1:svp)*diag(diagS(1:svp)-1/mu)*V(:,1:svp)';

        if norm(A_hat - temp_A, 'fro') < tolProj && norm(E_hat - temp_E, 'fro') < tolProj
            primal_converged = true;
        end
        A_hat = temp_A;
        E_hat = temp_E;
        primal_iter = primal_iter + 1;
        total_svd = total_svd + 1;

    end

    Z = D - A_hat - E_hat;
    Y = Y + mu*Z;
    mu = rho * mu;

    %% stop Criterion
    stopCriterion = norm(Z, 'fro') / dnorm;
    if stopCriterion < tol
        converged = true;
    end

    disp(['Iteration' num2str(iter) ' #svd ' num2str(total_svd) ' r(A) ' num2str(svp)...
        ' |E|_0 ' num2str(length(find(abs(E_hat)>0)))...
        ' stopCriterion ' num2str(stopCriterion)]);

    if ~converged && iter >= maxIter
        disp('Maximum iterations reached') ;
        converged = 1 ;
    end
end

if nargin == 5
    fclose(fid);
end


function determ = ris_determinant ( n )

%*****************************************************************************80
%
%% RIS_DETERMINANT returns the determinant of the Ris matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM the determinant.
%
  top = 1.0;
  for i = 1 : n
    for j = i + 1 : n
      top = top * ( 4 * ( i - j ) * ( i - j ) );
    end
  end

  bottom = 1.0;
  for i = 1 : n
    for j = 1 : n
      bottom = bottom * ( 3 + 2 * n - 2 * i - 2 * j );
    end
  end

  determ = top / bottom;

  return
end


% Procedure to get nodal stresses using Extrapolation method
function [sigma] = Extrapolation(stressGP)

%--------------------------------------------------------------------------
% Purpose : Extrapolating stress at Gaussian points to get nodal stress
%           values
%
% Synopsis : sigma = extrapolation(stressGP)
%
% Variable Description :
%       sigma - Stress at nodal points
%       stressGP - Stressat gaussian points
%--------------------------------------------------------------------------
% Extrapolation matrix obtained from Shape Functions
% Stress points are at (-1,-1),(-1,1),(1,1),1,-1)
explmt = [1+sqrt(3)/2    -1/2          1-sqrt(3)/2    -1/2;
           -1/2       1-sqrt(3)/2          -1/2    1+sqrt(3)/2 ;
          1-sqrt(3)/2    -1/2          1+sqrt(3)/2    -1/2;
           -1/2       1+sqrt(3)/2         -1/2     1-sqrt(3)/2 ] ;

sigmax = explmt*stressGP(:,1) ;
sigmay = explmt*stressGP(:,2) ;
sigmaxy = explmt*stressGP(:,3) ;

sigma = [sigmax sigmay sigmaxy];


function z = makebw(or,ph)
% function z = makebw(or,ph)
%
% Create BWT wavelets
% Version 1.2
%
% Arguments:
%  or: Orientation { -1 (DC), 0, 45, 90, 135 }
%  ph: Phase { 0 (odd), 90 (even) }
%
% Result:
%  z: The selected wavelet, or NaN if the parameters are not recognized
%
% Citation:
%  Willmore B, Prenger RJ, Wu MC and Gallant JL (2008). The Berkeley
%  Wavelet Transform: A biologically-inspired orthogonal wavelet transform.
%  Neural Computation 20:6, 1537-1564
%
% The article is available at:
%  <http://dx.doi.org/10.1162/neco.2007.05-07-513>
%
% Copyright (c) 2008 Ben Willmore
%
% Permission is hereby granted, free of charge, to any person
% obtaining a copy of this software and associated documentation
% files (the "Software"), to deal in the Software without
% restriction, including without limitation the rights to use,
% copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following
% conditions:
%
% The above copyright notice and this permission notice shall be
% included in all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
% EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
% OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
% NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
% HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
% WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
% OTHER DEALINGS IN THE SOFTWARE.

z = nan;

if (or==-1)
    z = ones(3,3)/sqrt(9);

elseif (or==0)
    if (ph==0)
        z = [-1 0 1; -1 0 1; -1 0 1]/sqrt(6);
    elseif (ph==90)
        z = [-1 2 -1; -1 2 -1; -1 2 -1]/sqrt(18);
    end

elseif (or==45)
    if (ph==0)
        z = [-1 1 0; 1 0 -1; 0 -1 1]/sqrt(6);
    elseif (ph==90)
        z = [-1 -1 2; -1 2 -1; 2 -1 -1]/sqrt(18);
    end

elseif (or==90)
    if (ph==0)
        z = [-1 -1 -1; 0 0 0; 1 1 1]/sqrt(6);
    elseif (ph==90)
        z = [-1 -1 -1; 2 2 2; -1 -1 -1]/sqrt(18);
    end

elseif (or==135)
    if (ph==0)
        z = [0 -1 1; 1 0 -1; -1 1 0]/sqrt(6);
    elseif (ph==90)
        z = [2 -1 -1; -1 2 -1; -1 -1 2]/sqrt(18);
    end

end

if (isnan(z))
    disp('Wavelet parameters not recognized');
end


%% RATE OF CONVERGENCE OF MIXED FINITE ELEMENT METHOD (RT0-P0) FOR POISSON EQUATION
%
% This example is to show the rate of convergence of RT0-P0 mixed finite
% element approximation of the Poisson equation on the unit square with the
% following boundary conditions:
%
% - pure Dirichlet boundary condition.
% - Pure Neumann boundary condition.
% - mxied boundary condition.
%
% The basis, data structure and numerical test is summarized in <a
% href="matlab:ifem PoissonRT0femrate">PoissonRT0femrate</a>.
%
% The optimal rates of convergence for u and sigma are observed, namely,
% 1st order for L2 norm of u, L2 norm of sigma and H(div) norm of sigma.
% The 2nd order convergent rates between two discrete functions ||uI-uh||
% and ||sigmaI-sigmah|| are known as superconvergence.
%
% Triangular preconditioned GMRES (default solver) and Uzawa preconditioned
% CG converges uniformly in all cases. Traingular preconditioner is two
% times faster than PCG although GMRES is used.
% See also PoissonBDM1mfemrate, Poissonfemrate, Poissonafemrate
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Setting
[node,elem] = squaremesh([0,1,0,1],0.25);
mesh = struct('node',node,'elem',elem);
option.L0 = 1;
option.maxIt = 4;
option.printlevel = 1;
option.elemType = 'RT0';
option.solver = 'tri';
pde = sincosNeumanndata;

%% Pure Neumann boundary condition.
mesh.bdFlag = setboundary(node,elem,'Neumann');
mfemPoisson(mesh,pde,option);

%% Pure Dirichlet boundary condition.
mesh.bdFlag = setboundary(node,elem,'Dirichlet');
mfemPoisson(mesh,pde,option);

%% Mix Dirichlet and Neumann boundary condition.
option.solver = 'uzawapcg';
mesh.bdFlag = setboundary(node,elem,'Dirichlet','~(x==0)','Neumann','x==0');
mfemPoisson(mesh,pde,option);


function loc_2d_shapes(NumOfSamples)
% Create Localized 2-d data from uniform distribution.
% Data are bount to the rectangle between points A(0,6) and B(4,10),
% a circle with center at C(7.5,2.5) and radius R = 2,
% and an isosceles triangle defined by the intesection points
% of the lines y=6, y=2x-5 and y=-2x+25.
% Every local set has the same number of samples (NumOfSamples/4).

clc;
input_dims = 2;

% Initialize Sample counters.
k1 = 0; k2 = 1; k3 = 0; k4 = 0;

% Initialize Data
Data = [7.5; 2.5;];

while size(Data,2)<NumOfSamples

In1 = 10*rand(input_dims,1);

% Apply Mask to input Data
if (In1(1,1)<4)&&(In1(1,1)>0)&&(In1(2,1)<10)&&(In1(2,1)>6)&&(k1<=NumOfSamples/4)
    Data = [Data In1];
    k1 = k1 + 1;
elseif norm(In1-[7.5; 2.5;])<=2&&(k2<=NumOfSamples/4)
    Data = [Data In1];
    k2 = k2 + 1;
elseif (In1(2,1)>6)&&(In1(2,1)<2*In1(1,1)-5)&&(In1(2,1)<-2*In1(1,1)+25)&&(k3<=NumOfSamples/4)
    Data = [Data In1];
    k3 = k3 + 1;
elseif (abs(In1(2,1)-In1(1,1))<.1)&&(In1(2,1)<5)&&(In1(2,1)>.5)&&(In1(1,1)<5)&&(In1(1,1)>.5)&&(k4<=NumOfSamples/4)
    Data = [Data In1];
    k4 = k4 + 1;
end
end

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

save local_uniform_2d Data;
plot(Data(1,:),Data(2,:),'.','MarkerSize',1);
grid on;
clear;