function [data_interleave] = interleaving(data_convolutional,Ncpc,Tx);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                                                                       %
%%     Name: interleaving.m                                              %
%%                                                                       %
%%     Description: This process is for interleave the bits to transmit  %
%%      and to make sure two consecutive bits are not modulated to       %
%%      the same frequency. It is another form to fight against          %
%%      frequency selective fading and burst errors.                     %
%%                                                                       %
%%                                                                       %
%%     Parameters:                                                       %
%%      Sequence of bits to interleave.                                  %
%%                                                                       %
%%     Result: It gives back the chain of bits, properly interleaved.    %
%%                                                                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% The size of the block to use to interleave the sequence is needed.
% This value is taken from table 223 of the standard.
% The value of the used block depends on the mapping that we want.

switch Ncpc
    case 1                          % for BPSK
        Ncbps=192;
    case 2                          % for QPSK
        Ncbps=384;
    case 4                          % for 16-QAM
        Ncbps=768;
    case 6                          % for 64-QAM
        Ncbps=1152;
end

s=ceil(Ncpc/2);

if Tx==1
  % In the following operations, it is necessary to consider the following values:
  % k-->index of the bit encoded BEFORE the first permutation
  % mk-->index of this bit BEFORE the second permutation and AFTER the first one
  % jk-->index after the SECOND permutation, just before the mapping of the sign.

    k = 0:Ncbps-1;
    mk = ((Ncbps/12)*mod(k,12))+floor(k/12);                     % First permutation
    jk = s*floor(mk/s)+mod(mk+Ncbps-floor(12*mk/Ncbps),s);       % Second permutation

    % Now I must arrange the indices to know in what order I must take the bits of the entering sequence.
    [a c] = sort(jk);

elseif Tx==0
  % Again, it is necessary to understand the values:
  % j-->index of the bit encoded BEFORE the first permutation
  % mj-->index of this bit BEFORE the second permutation and AFTER the first one
  % kj-->index after the SECOND permutation, just before the mapping of the sign.

    j = 0:Ncbps-1;
    mj = s*floor(j/s) + mod((j + floor(12*j/Ncbps)),s);          % First permutation
    kj = 12*mj-(Ncbps-1)*floor(12*mj/Ncbps);                     % Second permutation

    % The indices are ordered to know what must be taken.
    [a c]= sort(kj);

end

% finally the bits are rearranged.

 i = 1:Ncbps-1;
 data_interleave = zeros(1,Ncbps);
 data_interleave(i) = data_convolutional(c(i));


function [ x, y, w ] = rule04 ( )

%*****************************************************************************80
%
%% RULE04 returns the rule of degree 4.
%
%  Discussion:
%
%    Order 4 (6 pts)
%    1/6 data for 4-th order quadrature with 2 nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Output, real X(*), Y(*), the coordinates of the nodes.
%
%    Output, real W(*), the weights.
%
  x = [ ...
       0.00000000000000000000000000000000, ...
       0.00000000000000000000000000000000 ];
  y = [ ...
      -0.39011034927098671317419951524681, ...
       0.83747122925185483484415621413487 ];
  w = [ ...
       0.14699335257362380894667320570121, ...
       0.72352316251791601189863277931619E-01 ];

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve the following exercises:
%   A) A simulation of a DC motor.
%
% Help: Function prototype
% [y, t] = runge_kutta(f, y0, [t0 tfinal], timestep)
% where f is the function being integrated as dy/dt = f(t, y).
% y0 are the initial conditions
% t0: initial time
% tfinal: final time.
% h: time step for the calculations.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function simulate_motor_runge_kutta()
close all;
t0 = 0;
tfinal = 15;

[t, y] = runge_kutta(@motor, [0 0 0]', [t0 tfinal], 0.01);
y = y(:, 1:length(t));
plot(t, y(1,:), 'r'), hold
plot(t, y(2,:), 'g')
plot(t, y(3,:), 'b')
legend('Position theta (rad)', 'Speed (rad/s)', 'Current (A)')

figure,
plot(y(1,:), y(2,:), 'r')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function for Runge-Kutta.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = motor(t, x)
% We must return the solution of
% [dx1/dt; dx2/dt; dx3/dt dx4/dt]
Jm=500*1e-3; %kg*m2
R=0.566;%(Ohm)
L=0.172e-3;%(H)
Kb=1/(183*(pi/30)); % V/rad/s
Kp= 52.2/1000;
Bm = 0.001;
G=1;
tau = 0; % Nm
V = 5; % Volt

thetadd = (1/Jm)*(-Bm*x(2)+Kp*x(3)-tau/G);
currentd = (1/L)*(V-Kb*x(2)-R*x(3));

xd(1) = x(2);
xd(2) = thetadd;
xd(3) = currentd;
xd = xd(:);


% loss_hsp - hyperbolic secant loss function
%
% Copyright(c) 2009-2011 Ryota Tomioka
%              2009      Stefan Haufe
% This software is distributed under the MIT license. See license.txt
function [floss, gloss]=loss_hsp(zz, bb)

% floss = -sum(log(sech(bb-zz)./pi));
% gloss = tanh(zz-bb);

zz = zz-bb;
mz = abs(zz);

floss = sum(mz + log(exp(zz-mz)+exp(-zz-mz))-log(2/pi));
gloss = (exp(zz-mz)-exp(-zz-mz))./(exp(zz-mz)+exp(-zz-mz));


function [r,x,y,z] = elec_sphere_project(X,Y,Z,xo,yo,zo,varargin)

% elec_sphere_project - calculates spherical projections of electrode positions
%
% Usage: [r,x,y,z] = elec_sphere_project(X,Y,Z,xo,yo,zo,[estim],[plot])
%
% Notes:    The general formula for a sphere, with radius r is given by:
%
%           (x - xo)^2  +  (y - yo)^2  +  (z - zo)^2  =  r^2
%
%           This function takes arguments for cartesian co-ordinates
%           of X,Y,Z (assume Z > 0) and the center of the sphere (xo,yo,zo).
%           If (xo,yo,zo) are not provided, they are assumed (0,0,0).
%
%           Returned values are the fitted radius 'r' (constant)
%           and the (x,y,z) Cartesian co-ordinates on the projected sphere.
%
%           Options: estim - echo spherical radius estimates (default),
%                    plot  - plot the input/projected xyz on a sphere.
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:55 $

% Licence:  GNU GPL, no express or implied warranties
% History:  02/2002, Darren.Weber_at_radiology.ucsf.edu
%                    adapted from elec_fit_sph
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% initialise centroid, unless input parameters defined
if ~exist('xo','var') xo = 0; else, if isempty(xo) xo = 0; end, end
if ~exist('yo','var') yo = 0; else, if isempty(yo) yo = 0; end, end
if ~exist('zo','var') zo = 0; else, if isempty(zo) zo = 0; end, end

if (nargin>6),        estim = varargin{1};
else                  estim = 1;
end
if (nargin>7),        plot = varargin{2};
else                  plot = 0;
end

eegversion = '$Revision: 1.1 $';
fprintf('ELEC_SPHERE_PROJECT [v %s]\n',eegversion(11:15));
fprintf('...projecting electrodes to spherical points\n');
tic;

% Initialise r0 as a rough guess at the sphere radius
rX = (max(X) - min(X)) / 2;
rY = (max(Y) - min(Y)) / 2;
rZ =  max(Z) - zo;
r0 = mean([ rX rY rZ ]);

if isequal(estim,1),
    fprintf('...initial spherical radius   = %f\n', r0); end

% perform least squares estimate of spherical radius (r)
options = optimset('fminsearch');
[r,fval,exitflag,output] = fminsearch('elec_sphere_fit_optim',...
                           r0, options, X, Y, Z, xo, yo, zo);
%fprintf('\n%s%f\n', 'Iterations = ', output.iterations);
%fprintf('\n%s%d\n', 'Exit = ', exitflag);

if isequal(estim,1),
    fprintf('...estimated spherical radius = %f\n', r); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Find the projection point of X,Y,Z to the fitted sphere radius r

% Convert Cartesian X,Y,Z to spherical (radians)
theta = atan2( (Y-yo), (X-xo) );
phi = atan2( sqrt( (X-xo).^2 + (Y-yo).^2 ), (Z-zo) );
% do not recalc: r = sqrt( (X-xo).^2 + (Y-yo).^2 + (Z-zo).^2);

%   Recalculate X,Y,Z for constant r, given theta & phi.
R = ones(size(phi)) * r;
x = R .* sin(phi) .* cos(theta);
y = R .* sin(phi) .* sin(theta);
z = R .* cos(phi);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot the input & projected electrode positions on a sphere
if isequal(plot,1),
    figure('NumberTitle','off','Name','Electrode Placements');
    set(gca,'Projection','perspective');
    set(gca,'DataAspectRatio',[1 1 1]);

    hold on

    plot3(x,y,z,'b.');
    plot3(X,Y,Z,'ro');
    legend('input xyz','fitted sphere','Location','BestOutside');

    [Xs,Ys,Zs]=sphere;
    Xs = Xs * r;
    Ys = Ys * r;
    Zs = Zs * r;

    surf(Xs,Ys,Zs,...
        'EdgeColor','none',...
        'FaceColor',[0.7 0.7 0.7],...
        'FaceAlpha',0.4);
    view(2);
    rotate3d;

    axis tight; hold off;
end

t = toc; fprintf('...done (%6.2f sec).\n\n',t);

return


function y = vl_nnnormalizelp(x,dzdy,varargin)
%VL_NNNORMALIZELP  CNN Lp normalization
%   Y = VL_NNNORMALIZELP(X) normalizes in Lp norm each spatial
%   location in the array X:
%
%       Y(i,j,k) = X(i,j,k) / sum_q (X(i,j,q).^p + epsilon)^(1/p)
%
%   DZDX = VL_NNNORMALIZELP(X, DZDY) computes the derivative of the
%   function with respect to X projected onto DZDY.
%
%   VL_NNNORMALIZE(___, 'opts', val, ...) takes the following options:
%
%   `p`:: 2
%      The exponent of the Lp norm. Warning: currently only even
%      exponents are supported.
%
%   `epsilon`: 0.01
%      The constant added to the sum of p-powers before taking the
%      1/p square root (see the formula above).
%
%   See also: VL_NNNORMALIZE().

opts.epsilon = 1e-2 ;
opts.p = 2 ;
opts = vl_argparse(opts, varargin) ;

massp = (sum(x.^opts.p,3) + opts.epsilon) ;
mass = massp.^(1/opts.p) ;
y = bsxfun(@rdivide, x, mass) ;

if nargin < 2 || isempty(dzdy)
  return ;
else
  dzdy = bsxfun(@rdivide, dzdy, mass) ;
  y = dzdy - bsxfun(@times, sum(dzdy .* x, 3), bsxfun(@rdivide, x.^(opts.p-1), massp)) ;
end


function W = getPCAWhite( fv_data, targetDim )
%GETPCAWHITE Compute PCA-Whitening matrix
%   Dependency on pca() function of MATLAB Statistics Toolbox

    % pca() expects data to be row-wise, hence transposed fv_data
    [pca_proj, ~, pca_eigvals] = ...
                   pca(fv_data', 'NumComponents', targetDim, ...
                    'Centered',false);

    whiteMat =  diag(1./sqrt(pca_eigvals(1:targetDim) + 1e-5));
    W = single(whiteMat * pca_proj');

end


% Calculate statistics of a rate map that depend on probability distribution function (PDF)
%
% Calculates information, sparsity and selectivity of a rate map. Calculations are done
% according to 1993 Skaggs et al. "An Information-Theoretic Approach to Deciphering the Hippocampal Code"
% paper. Another source of information is 1996 Skaggs et al. paper called
% "Theta phase precession in hippocampal neuronal populations and the compression of temporal sequences".
%
%  USAGE
%   [information, sparsity, selectivity] = analyses.mapStatsPDF(map)
%   map             Structure with rate map, output of analyses.map
%   information     Structure with fields:
%       rate        information rate [bits/sec]
%       content     Spatial information content [bits/spike]
%   sparsity        Sparsity value
%   selectivity     Selectivity value
%
%  SEE
%   See also analyses.map
%
function [information, sparsity, selectivity] = mapStatsPDF(map)
    if ~isstruct(map)
        error('BNT:arg', 'Incorrect argument. Map should be a structure. You are probably relying on old code when mapStatsPDF accepted map as matrix.');
    end
    T = nansum(nansum(map.time)); % overall trial duration
    posPDF = map.time / (T+eps); % probability of animal being in bin x:
                                 % duration of time animal spent in the bin divided by overall trial duration.

    meanrate = nansum(nansum(map.z .* posPDF));
    meansquarerate = nansum(nansum( (map.z .^ 2) .* posPDF ));
    if meansquarerate == 0
       sparsity = NaN;
    else
        sparsity = meanrate^2 / meansquarerate;
    end

    maxrate = nanmax(nanmax(map.z));
    if meanrate == 0;
       selectivity = NaN;
       information.content = nan;
       information.rate = nan;
    else
       selectivity = maxrate / meanrate;
       logArg = map.z / meanrate;
       logArg(logArg < 1) = 1;

       information.rate = nansum(nansum(posPDF .* map.z .* log2(logArg)));
       information.content = information.rate / meanrate;
    end
end


function gauss = get_gaussian_kernel(Nsigmas, sigma_in_pix)
% generate gaussian kernel with given sigmas
    ndims = numel(sigma_in_pix);
    width = 2 * floor(Nsigmas .* sigma_in_pix) + 1;
    hw = (width - 1)/2;

    sigma_in_pix(sigma_in_pix == 0) = 1;
    if ndims == 3
        -hw(1):hw(1)
        hw(2)
        hw(3)
        [n1, n2, n3] = ndgrid(-hw(1):hw(1), -hw(2):hw(2), -hw(3):hw(3));
        size(n1)
        weight = exp(-n1.^2/(2*sigma_in_pix(1)^2) - n2.^2/(2*sigma_in_pix(2)^2) - n3.^2/(2*sigma_in_pix(3)^2) );
    elseif ndims == 2
        [n1, n2] = ndgrid(-hw(1):hw(1), -hw(2):hw(2));
        weight = exp(-n1.^2/(2*sigma_in_pix(1)^2) - n2.^2/(2*sigma_in_pix(2)^2));
    end

    gauss = weight / sum(weight(:));
end


function [smaIn, eIn, hHat, sHat, smaOut, eOut, oHat] = computeMultiFlyByParameters(vInfIn, vInfOut, gmu)
%computeFlyByParameters Summary of this function goes here
%   Detailed explanation goes here

    normVInfIn = sqrt(sum(abs(vInfIn).^2,1));
    normVinfOut = sqrt(sum(abs(vInfOut).^2,1));

    sHat = bsxfun(@rdivide,vInfIn,normVInfIn);
    energyIn = normVInfIn.^2./2;
    smaIn = -gmu./(2*energyIn);

    oHat = bsxfun(@rdivide,vInfOut,normVinfOut);
    energyOut = normVinfOut.^2./2;
    smaOut = -gmu./(2*energyOut);

    crossSO = cross(sHat, oHat);
    normCrossSO = sqrt(sum(abs(crossSO).^2,1));
    hHat = bsxfun(@rdivide,crossSO,normCrossSO);
    delta = acos(dot(sHat,oHat)); %may need the dim argument

    e1Eqn = @(e2) 1+(smaOut./smaIn).*(e2-1);
    f = @(e2) delta - asin(1./e1Eqn(e2)) - asin(1./e2);
    lb = 1.0 * ones(1,length(delta));
    ub = 1000000000 * ones(1,length(delta));

    [eOut, ~, ~] = bisection(f,lb,ub, [], 1E-8);
    eIn = e1Eqn(eOut);
end


function [reg_kth_largest,num_regions] = get_kth_islands(img_b,k_th)
% extract the kth largest image island from a binary image, and
% give the total number of islands in this binary image
    img_label = bwlabel(img_b);
    reg_size = regionprops(img_label,'Area');
    reg = cat(1,reg_size.Area);
    num_regions = length(reg);
    if num_regions == 0
        error('cannot find any image island');
    end
    reg_sorted = sort(reg,'descend');
    ind = find(reg == reg_sorted(k_th));
    reg_kth_largest = ismember(img_label,ind);
end


% Fig. 6.29   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clear all;
%close all;
clf

num=[1 1];
den=conv([1 0],[0.1 -1]);
rlocus(num,den);

axis([-8 12 -10 10]);
axis equal
grid;
title('Fig. 6.29 Root locus for Example 6.10');


function out=atanh(x)

out=(log(1+x)-log(1-x))/2;


function [A_K_I,A_K_F,A_F_I,A_Rfl_F,A_Rfr_F,A_down_up] = Rotation_Matrices(phi,chi,delta)
%% Rotation from rear axle fixed coordinate system into vehicle fixed coordinate system
% Input parameters:
% phi       [rad]    Vehicle Rotation Angles [Phi Theta Psi] [3x1]
% chi       [rad]    Road Cardan Angles [chi_x chi_y Psi] [3x1]
% delta     [rad]    Steering Angles [deltaFL deltaFR 0 0] [1x4]
% Output parameters:
% A_K_I     [---]    Transformation Matrix A_K_I [3x3]
% A_K_F     [---]    Transformation Matrix A_K_F [3x3]
% A_F_I     [---]    Transformation Matrix A_F_I [3x3]
% A_Rfl_F   [---]    Transformation Matrix A_Rfl_F [3x3]
% A_Rfr_F   [---]    Transformation Matrix A_Rfr_F [3x3]
% A_down_up [---]    Transformation Matrix W := A_down_up [3x3]

%% Pre-Computation of trigonometric values
cphi=cos(phi);
sphi=sin(phi);
cchi=cos(chi);
schi=sin(chi);

%% Definition of rotation matrices

% rotation matrix from z-up coordinate system to z-down coordinate system
A_down_up = reshape([1 0 0,0 -1 0,0 0 -1],[3,3]);

% rotation matrix from inertial coordinate system to vehicle fixed coordinate system
A_K_I     = reshape([cphi(3)*cphi(2),-cphi(1)*sphi(3)+cphi(3)*sphi(2)*sphi(1),sphi(3)*sphi(1)+cphi(3)*cphi(1)*sphi(2),cphi(2)*sphi(3),cphi(3)*cphi(1)+sphi(3)*sphi(2)*sphi(1),-cphi(3)*sphi(1)+cphi(1)*sphi(3)*sphi(2),-sphi(2),cphi(2)*sphi(1),cphi(2)*cphi(1)],[3,3]);

% rotation matrix from rear axle fixed coordinate system to vehicle fixed coordinate system without road inclination
% A_K_F     = reshape([cphi(2),sphi(2)*sphi(1),cphi(1)*sphi(2),0,cphi(1),-sphi(1),-sphi(2),cphi(2)*sphi(1),cphi(2)*cphi(1)],[3,3]);

% rotation matrix from rear axle fixed coordinate system to vehicle fixed coordinate system over inertial coordinate system with road inclination
A_K_F     = reshape([cos(phi(2)-chi(2)),sin(phi(2)-chi(2))*sphi(1),sin(phi(2)-chi(2))*cphi(1),-sin(-chi(2)+phi(2))*schi(1),cchi(1)*cphi(1)+cphi(2)*cchi(2)*schi(1)*sphi(1)+sphi(2)*schi(1)*schi(2)*sphi(1),-cchi(1)*sphi(1)+cphi(2)*cchi(2)*cphi(1)*schi(1)+cphi(1)*sphi(2)*schi(1)*schi(2),-sin(-chi(2)+phi(2))*cchi(1),-cphi(1)*schi(1)+cphi(2)*cchi(1)*cchi(2)*sphi(1)+cchi(1)*sphi(2)*schi(2)*sphi(1),schi(1)*sphi(1)+cphi(2)*cchi(1)*cchi(2)*cphi(1)+cchi(1)*cphi(1)*sphi(2)*schi(2)],[3,3]);

% rotation matrix from inertial coordinate system to rear axle fixed coordinate system
A_F_I     = reshape([cphi(3)*cchi(2),-cchi(1)*sphi(3)+cphi(3)*schi(1)*schi(2),sphi(3)*schi(1)+cphi(3)*cchi(1)*schi(2),cchi(2)*sphi(3),cphi(3)*cchi(1)+sphi(3)*schi(1)*schi(2),-cphi(3)*schi(1)+cchi(1)*sphi(3)*schi(2),-schi(2),cchi(2)*schi(1),cchi(1)*cchi(2)],[3,3]);

% rotation matrix from rear axle fixed coordinate system to wheel fixed coordinate systems of front axle
A_Rfl_F   = reshape([cos(delta(1)),-sin(delta(1)),0,sin(delta(1)),cos(delta(1)),0,0,0,1],[3,3]);
A_Rfr_F   = reshape([cos(delta(2)),-sin(delta(2)),0,sin(delta(2)),cos(delta(2)),0,0,0,1],[3,3]);

end


function [sigma,bnd] = bdsqr(alpha,beta)

% BDSQR: Compute the singular values and bottom element of
%        the left singular vectors of a (k+1) x k lower bidiagonal
%        matrix with diagonal alpha(1:k) and lower bidiagonal beta(1:k),
%        where length(alpha) = length(beta) = k.
%
% [sigma,bnd] = bdsqr(alpha,beta)
%
% Input parameters:
%   alpha(1:k)   : Diagonal elements.
%   beta(1:k)    : Sub-diagonal elements.
% Output parameters:
%   sigma(1:k)  : Computed eigenvalues.
%   bnd(1:k)    : Bottom elements in left singular vectors.

% Below is a very slow replacement for the BDSQR MEX-file.

% warning('PROPACK:NotUsingMex','Using slow matlab code for bdsqr.')
k = length(alpha);
if min(size(alpha)') ~= 1  | min(size(beta)') ~= 1
  error('alpha and beta must be vectors')
elseif length(beta) ~= k
  error('alpha and beta must have the same lenght')
end
B = spdiags([alpha(:),beta(:)],[0,-1],k+1,k);
[U,S,V] = svd(full(B),0);
sigma = diag(S);
bnd = U(end,1:k)';


% test for heat diffusion on meshes

% load mesh
name = 'mushroom';
name = 'fandisk';
name = 'lion-head';
name = 'bunny';
name = 'skull';
name = 'horse';
name = 'nefertiti';
name = 'elephant-50kv';
options.name = name;
[vertex,face] = read_off([name '.off']);

rep = 'results/mesh-smoothing/';
if not(exist(rep))
    mkdir(rep);
end

nvert = size(vertex,2);
nface = size(face,2);

laplacian_type = 'distance';
laplacian_type = 'combinatorial';
laplacian_type = 'conformal';

%% symmetric laplacian
if not(strcmp(laplacian_type,'conformal'))
    options.symmetrize = 1;
    options.normalize = 0;
    L0 = compute_mesh_laplacian(vertex,face,laplacian_type,options);
    G0 = compute_mesh_gradient(vertex,face,laplacian_type,options);
    disp(['Error (should be 0): ' num2str(norm(L0-G0'*G0, 'fro')) '.']);
    options.normalize = 1;
    L1 = compute_mesh_laplacian(vertex,face,laplacian_type,options);
    G1 = compute_mesh_gradient(vertex,face,laplacian_type,options);
    disp(['Error (should be 0): ' num2str(norm(L1-G1'*G1, 'fro')) '.']);
end

%% un-symmetric laplacian
options.symmetrize = 0;
options.normalize = 1;
disp('--> Computing laplacian');
L = compute_mesh_laplacian(vertex,face,laplacian_type,options);

%% heat diffusion flow
Tlist = [0 10 40 200];
options.dt = 0.3;
clf;
for i=1:length(Tlist)
    options.Tmax = Tlist(i);
    vertex1 = perform_mesh_heat_diffusion(vertex,face,L,options);
    % display
    subplot(1,length(Tlist),i);
    plot_mesh(vertex1,face,options);
    shading interp; camlight; axis tight;
end
saveas(gcf, [rep name '-smoothing-' laplacian_type '.png'], 'png');

%% quadratic regularization
clf;
for i=1:length(Tlist)
    At = speye(nvert) + Tlist(i)*L;
    vertex1 = (At\vertex');
    % display
    subplot(1,length(Tlist),i);
    plot_mesh(vertex1,face,options);
    shading interp; camlight; axis tight;
end
saveas(gcf, [rep name '-quadreg-' laplacian_type '.png'], 'png');

%% non linear L1 minimization
options.symmetrize = 1;
options.normalize = 0;
disp('--> Computing gradient');
G = compute_mesh_gradient(vertex,face,laplacian_type,options);
Tlist = [0 0.01 0.02 0.05 0.1 0.2];
clf;
for i=1:length(Tlist)
    t = Tlist(i);
    vertex1 = vertex;
    options.niter = 500;
    options.lambda_max = t*2;
    options.lambda_min = t;
    if t>0
    for k=1:3
        [a,tmp,E] = perform_analysis_regularization(vertex(k,:)', G, options);
        vertex1(k,:) = a(:)';
    end
    end
    % display
    subplot(2,ceil(length(Tlist)/2),i);
    plot_mesh(vertex1,face,options);
    shading interp; camlight; axis tight;
end
saveas(gcf, [rep name '-l1reg-' laplacian_type '.png'], 'png');


function H=circle(center,radius,NOP,style)
%---------------------------------------------------------------------------------------------
% H=CIRCLE(CENTER,RADIUS,NOP,STYLE)
% This routine draws a circle with center defined as
% a vector CENTER, radius as a scaler RADIS. NOP is
% the number of points on the circle. As to STYLE,
% use it the same way as you use the rountine PLOT.
% Since the handle of the object is returned, you
% use routine SET to get the best result.
%
%   Usage Examples,
%
%   circle([1,3],3,1000,':');
%   circle([2,4],2,1000,'--');
%
%   Zhenhai Wang <zhenhai@ieee.org>
%   Version 1.00
%   December, 2002
%---------------------------------------------------------------------------------------------

if (nargin <3),
 error('Please see help for INPUT DATA.');
elseif (nargin==3)
    style='b-';
end;
THETA=linspace(0,2*pi,NOP);
RHO=ones(1,NOP)*radius;
[X,Y] = pol2cart(THETA,RHO);
X=X+center(1);
Y=Y+center(2);
H=plot(X,Y,style);
axis square;


function [B,EV,ED] = biharmonic_embedding(V,F,dim,p);
  % [B,EV,ED] = biharmonic_embedding(V,F);
  %
  % Takes a mesh (V,F) and returns an embedding using the spectrum of the
  % biharmonic operator. Then the biharmonic distance between two points i and
  % j can be computed as the euclidean distance between B(i,:) and B(j,:),
  % namely:
  %   dist_ij = sqrt(sum((B(i,:)-B(j,:)).^2,2));
  %
  % Input:
  %   V  vertex list
  %   F  face list
  %   dim requested dimension of the embedding
  %   Optional:
  %     p  exponent above eigen values
  %       0.5  "semi-harmonic" embedding
  %       1  commute time embedding, "harmonic"
  %       2  biharmonic {default}
  %       3  "triharmonic" embedding
  % Output:
  %   B  biharmonic embedding
  %   EV  eigenvectors used in embedding
  %   ED  eigenvalues used in embedding
  %

  % if dimension is not specfied use 4
  if(~exist('dim','var'))
    dim = 4;
  end

  if(~exist('p','var'))
    p = 2;
  end

  % get cotangent matrix
  L = cotmatrix(V,F);
  % This should be better, but yaron seemed to use barycentric
  %M = massmatrix(V,F);
  M = massmatrix(V,F,'barycentric');

  % get dim+1 smallest magnitude eigenvalues and corresponding vectors
  [EV,ED] = eigs(L,M,dim+1,'sm');
  EV = EV(:, 2:end);
  ED = ED(2:end, 2:end);

  % This is not exactly the same, essentially it removes the mass matrix and
  % multiplies everything by a factos of -2
  % % This also works, because of the sign change in the eigenvalues matlab
  % % reverses the output order so 0.0 is the last eigenvalue
  % [EV,ED] = eigs(-2*L,M./sum(M(:)),dim+1,'sm');
  % EV = EV(:, 1:end-1);
  % ED = ED(1:end-1, 1:end-1);

  %  divide each eigenvector by corresponding eigenvalue
  %  divide the power by 2 first because it will appear in the denominator of
  %  distance computation *outside* the squared difference see (4) and (11)
  B = EV * (inv(abs(ED))^(p/2));
end


function varargout = drawCube(cube, varargin)
%DRAWCUBE Draw a 3D centered cube, eventually rotated.
%
%   drawCube(CUBE)
%   Displays a 3D cube on current axis. CUBE is given by:
%   [XC YC ZC SIDE THETA PHI PSI]
%   where (XC, YC, ZC) is the CUBE center, SIDE is the length of the cube
%   main sides, and THETA PHI PSI are angles representing the cube
%   orientation, in degrees. THETA is the colatitude of the cube, between 0
%   and 90 degrees, PHI is the longitude, and PSI is the rotation angle
%   around the axis of the normal.
%
%   CUBE can be axis aligned, in this case it should only contain center
%   and side information:
%   CUBE = [XC YC ZC SIDE]
%
%   The function drawCuboid is closely related, but uses a different angle
%   convention, and allows for different sizes along directions.
%
%   Example
%   % Draw a cube with small rotations
%     figure; hold on;
%     drawCube([10 20 30  50  10 20 30], 'FaceColor', 'g');
%     axis equal;
%     view(3);
%
%   See also
%   meshes3d, polyhedra, createCube, drawCuboid
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-06-29,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

% default values
theta = 0;
phi   = 0;
psi   = 0;

%% Parses the input
if nargin == 0
    % no input: assumes cube with default shape
    xc = 0;	yc = 0; zc = 0;
    a = 1;

else
    % one argument: parses elements
    xc  = cube(:,1);
    yc  = cube(:,2);
    zc  = cube(:,3);
    a   = cube(:,4);

    % parses orientation if present
    k   = pi / 180;
    if size(cube, 2) >= 6
        theta = cube(:,5) * k;
        phi   = cube(:,6) * k;
    end
    if size(cube, 2) >= 7
        psi   = cube(:,7) * k;
    end
end

%% Compute cube coordinates

% create unit centered cube
[v, f] = createCube;
v = bsxfun(@minus, v, mean(v, 1));

% convert unit basis to cube basis
sca     = createScaling3d(a);
rot1    = createRotationOz(psi);
rot2    = createRotationOy(theta);
rot3    = createRotationOz(phi);
tra     = createTranslation3d([xc yc zc]);

% concatenate transforms
trans   = tra * rot3 * rot2 * rot1 * sca;

% transform mesh vertices
[x, y, z] = transformPoint3d(v, trans);

%% Process output
if nargout == 0
    % no output: draw the cube
    drawMesh([x y z], f, varargin{:});

elseif nargout == 1
    % one output: draw the cube and return handle
    varargout{1} = drawMesh([x y z], f, varargin{:});

elseif nargout == 3
    % 3 outputs: return computed coordinates
    varargout{1} = x;
    varargout{2} = y;
    varargout{3} = z;
end


 function f = nufft_diric(k, N, K, use_true_diric)
%function f = nufft_diric(k, N, K, use_true_diric)
% "regular fourier" Dirichlet-function WITHOUT phase
% diric(t) = sin(pi N t / K) / ( N * sin(pi t / K) )
%	\approx sinc(t / (K/N))
% in:
%	k [...]		sample locations (unitless real numbers)
%	N		signal length
%	K		DFT length
%	use_true_diric	1 = use true Diric function.
%			(default is to use sinc approximation)
% out:
%	f [...]		corresponding function values
%
% Copyright 2001-12-8, Jeff Fessler, The University of Michigan

% default is to plot
if nargin < 3
	help(mfilename)
	kmax = 2 * (10 + 1 * 4);
	k = linspace(-kmax,kmax,201);
	ki = [-kmax:kmax];
	N = 32;
	K = 2*N;
	g = nufft_diric(k, N, K, 1);
	gi = nufft_diric(ki, N, K, 1);
	s = nufft_diric(k, N, K);
	dm = diric((2*pi/K)*k,N);
	plot(k, g, 'y-', k, s, 'c-', k, dm, 'r-', ki, gi, '.'), axis tight
	legend('nufft diric', 'sinc', 'matlab diric')
	xlabel k, ylabel diric(k)
	printf('max %% difference = %g', max_percent_diff(g,s))
	return
end

if nargin < 4
	use_true_diric = logical(0);
end

% diric version
if use_true_diric
	t = (pi/K) * k;
	f = sin(t);
	i = abs(t) > 1e-12;	% nonzero argument
	f(i) = sin(N*t(i)) ./ (N * f(i));
	f(~i) = 1;

% sinc version
else
%	f = sinc(k / (K/N));
	f = nufft_sinc(k / (K/N));
end

% this is faster than matlab's sinc because it does not use "find"
function y = nufft_sinc(x)
y = ones(size(x));
%i = abs(x) > 1e-12;
i = x ~= 0;
x = x(i);
y(i) = sin(pi*x) ./ (pi*x);


function a = r8gd_indicator ( n, ndiag, offset )

%*****************************************************************************80
%
%% R8GD_INDICATOR sets up a R8GD indicator matrix.
%
%  Discussion:
%
%    The R8GD storage format is suitable for matrices whose only nonzero entries
%    occur along a few diagonals, but for which these diagonals are not all
%    close enough to the main diagonal for band storage to be efficient.
%
%    In that case, we assign the main diagonal the offset value 0.
%    Each successive superdiagonal gets an offset value 1 higher, until
%    the highest superdiagonal (the A(1,N) entry) is assigned the offset N-1.
%    Similarly, the subdiagonals are assigned offsets of -1 through -(N-1).
%
%    Now, assuming that only a few of these diagonals contain nonzeros,
%    then for the I-th diagonal to be saved, we stored its offset in
%    OFFSET(I), and its entries in column I of the matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer NDIAG, the number of diagonals of the matrix
%    that are stored in the array.
%    NDIAG must be at least 1, and no more than 2 * N - 1.
%
%    Input, integer OFFSET(NDIAG), the offsets for the diagonal storage.
%
%    Output, real A(N,NDIAG), the R8GD matrix.
%
  fac = 10^( i4_log_10 ( n ) + 1 );

  for i = 1 : n
    for diag = 1 : ndiag
      j = i + offset(diag);
      if ( 1 <= j & j <= n )
        a(i,diag) = fac * i + j;
      else
        a(i,diag) = 0.0;
      end
    end
  end

  return
end


function coeffs = alias(coeffs, m)
%ALIAS   Alias Fourier coefficients on equally spaced grid.
%   ALIAS(C, M) aliases the Fourier coefficients stored in the column vector C
%   to have length M. If M > LENGTH(C), the coefficients are padded with zeros.
%   If C is a matrix of coefficients, each of the columns is aliased to length
%   M.
%
% See also PROLONG.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Get the number of coefficients.
n = size(coeffs, 1);

% Pad with zeros:
if ( m > n )

    k = ceil((m-n)/2);
    z = zeros(k, size(coeffs, 2));

    % Need to handle the odd vs. even case separately.
    if ( ~mod(n, 2) ) % n even.

        % First, account for the asymmetry in the coefficients when n is even.
        % This will account for the cos(N/2) coefficient, which is stored
        % in the coeffs(n,:) entry, using properties of the complex
        % exponential.
        coeffs = [ coeffs(1,:)/2; coeffs(2:n,:); coeffs(1,:)/2 ];
        coeffs = [ z; coeffs; z(1:end-1,:) ];

        % Next, check if m is odd. If it is, then coeffs is too long and we
        % need to remove the first row (the lowest degree
        % coefficients).
        if ( mod(m, 2) ) % m odd.
            coeffs = coeffs(2:end,:);
        end

    else % n odd.

        % There is no asymmetry in the coefficients, just pad them.
        coeffs = [ z; coeffs; z ];

        % Only need to check if m is even, in which case coeffs is too
        % long and we need to remove the last row (the highest degree
        % coefficients).
        if ( ~mod(m, 2) ) % m even.
            coeffs = coeffs(1:end-1,:);
        end
    end

    return

end

% If the number of coefficients is even then we extend them by one so they
% are odd by exploiting the symmetry property.  This makes the code below a
% little cleaner since fewer cases need to be handled.
if ( ~mod(n, 2) ) % n even.
    coeffs(1,:) = 0.5*coeffs(1,:);
    coeffs = [ coeffs; coeffs(1,:) ];
    n = n + 1;
end

% Need to handle the odd and even case (for m) differently.
if ( mod(m, 2) ) % m is odd.

    if ( m == 1 )

        % Reduce to a single point:
        constCoeffs = coeffs((n-1)/2+1,:);
        posCoeffs = coeffs((n-1)/2:-1:1,:);
        negCoeffs = coeffs((n-1)/2+2:n,:);
        e = ones(1, ceil((n-1)/2));
        e(1:2:end) = -1;
        coeffs = constCoeffs + (e*posCoeffs + e*negCoeffs);

    else

        m2 = (m-1)/2;
        n2 = (n-1)/2;
        % Extract coefficients:
        aliasedCoeffs = coeffs(n2-m2+1:n2+m2+1,:);

        % The code below aliases the coefficients from the higher modes
        % onto the lower modes. The principle behind the formula is figure
        % out which of the higher Fourier modes are indistinguishable from
        % the lower Fouirer modes on the grid consisting of m equally
        % spaced points from [-1,1). In general, when m and n are odd the
        % following will be equal for j=-(n-1)/2 to -(m+1)/2
        % exp(1i*pi*j*x) = sgn*exp(1i*pi*k*x) where
        % k = mod(j+(m+1)/2, -m) + (m-1)/2 and sgn = (-1)^mod(j+k, 2);
        for j = -n2:-m2-1
            k = mod(j+m2+1, -m) + m2;
            coeffIndexK = k + m2 + 1;  % Index into aliasedCoeffs for mode k.
            coeffIndexJ = j + n2 + 1;  % Index into coeffs for mode j.
            sgn = (-1)^mod(j+k, 2);
            aliasedCoeffs(coeffIndexK,:) = aliasedCoeffs(coeffIndexK,:) + sgn*coeffs(coeffIndexJ,:);
            coeffIndexK = -k + m2 + 1;
            coeffIndexJ = -j + n2 + 1;
            aliasedCoeffs(coeffIndexK,:) = aliasedCoeffs(coeffIndexK,:) + sgn*coeffs(coeffIndexJ,:);
        end
        coeffs = aliasedCoeffs;

    end

else % m is even.

    m2 = m/2;
    n2 = (n-1)/2;

    % Put the coefficient for the cos(m/2*pi*x) in the first entry of the
    % coefficient vector.  This corresopnds to the exp(-1i*pi*m/2*x).
    aliasedCoeffs = coeffs(n2+1-m2:n2+m2,:);
    % Extend the aliased cofficient vector so it is symmetric.  This allows
    % us to easily account for aliasing on the exp(-1i*pi*m/2*x) and
    % exp(1i*pi*m/2*x) term in the code below without the need for a
    % special if check.  The negative allows contribution for sin(m/2*pi*x)
    % to be removed.
    aliasedCoeffs = [ aliasedCoeffs; -aliasedCoeffs(1,:) ];

    % Follow a similar structure to the odd m case above.  The main
    % difference is that the higher order modes do not change sign when
    % aliased onto an even point grid.
    for j = -n2:-m2
        k = mod(j+m2,-m) + m2;
        coeffIndexK = k + m2 + 1;  % Index into aliasedCoeffs for mode k.
        coeffIndexJ = j + n2 + 1;  % Index into coeffs for mode j.
        aliasedCoeffs(coeffIndexK,:) = aliasedCoeffs(coeffIndexK,:) + coeffs(coeffIndexJ,:);
        coeffIndexK = -k + m2 + 1;
        coeffIndexJ = -j + n2 + 1;
        aliasedCoeffs(coeffIndexK,:) = aliasedCoeffs(coeffIndexK,:) + coeffs(coeffIndexJ,:);
    end
    % Collapse the aliased coefficient vector down to an even number of
    % terms by adding in the aliasing of the exp(1i*pi*m/2*x) terms to the
    % exp(-1i*pi*m/2*x).
    aliasedCoeffs(1,:) = aliasedCoeffs(1,:) + aliasedCoeffs(end,:);
    aliasedCoeffs(end,:) = [];

    coeffs = aliasedCoeffs;

end

end


%
%% PRIMES_SCRIPT.M
%
fprintf ( 1, '\n' );
fprintf ( 1, 'PRIMES_SCRIPT:\n' );
fprintf ( 1, '  MATLAB version.\n' );
tic;
s4 = sum ( isprime(3:2:10^4) ) + 1
t4 = toc
%
tic;
s5 = sum ( isprime(3:2:10^5) ) + 1
t5 = toc
%
%tic;
%s6 = sum ( isprime(3:2:10^6) ) + 1
%t6 = toc
%
tic;
p4 = length ( primes(10^4) )
t4 = toc;
%
tic;
p5 = length ( primes(10^5) )
t5 = toc;
%
tic;
p6 = length ( primes(10^6) )
t6 = toc;
output = fopen ( 'primes_report.txt', 'w' );
fprintf ( output, 'There are %d prime numbers up to 10^4.\n', s4 );
fprintf ( output, 'There are %d prime numbers up to 10^5.\n', s5 );
%fprintf ( output, 'There are %d prime numbers up to 10^6.\n', s6 );
fclose ( output );
%
%  Let's find out what files exist now.
%
ls
%
%  Terminate.
%
fprintf ( 1, '\n' );
fprintf ( 1, 'PRIMES_SCRIPT:\n' );
fprintf ( 1, '  Normal end of execution.\n' );


function [r,s,t,w] = threed_gauss(rule)
%-----------------------------------------------------------------------
%  threed_gauss.m - calculate Gauss integration points for tetrahedral
%                   elements
%
%  Copyright (c) 2002, Jeff Borggaard, Virginia Tech
%  Version: 1.0
%
%  Usage:    [r,s,t,w] = threed_gauss(rule)
%
%  Variables:     rule
%                        Number of Gauss points:
%                 r
%                        xi coordinate of Gauss points
%                 s
%                        eta coordinate of Gauss points
%                 t
%                        zeta coordinate of Gauss points
%                 w
%                        Gauss weights corresponding to (r,s,t)
%-----------------------------------------------------------------------

  if (rule == 1)
    % The following points correspond to a 1 point rule
    % Polynomials of degree 1 are integrated exactly
    r(1) = 0.25000000;  s(1) = 0.25000000;  t(1) = 0.25000000;

    w(1) = 0.16666666;  % (the volume of a regular tet is 1/6)

  elseif (rule == 4)
    % The following points correspond to a 4 point rule
    % Polynomials of degree 2 are integrated exactly
    r    = zeros(4,1);  s    = zeros(4,1);  t    = zeros(4,1);
    r(1) = 0.13819660;  s(1) = 0.13819660;  t(1) = 0.13819660;
    r(2) = 0.58541020;  s(2) = 0.13819660;  t(2) = 0.13819660;
    r(3) = 0.13819660;  s(3) = 0.58541020;  t(3) = 0.13819660;
    r(4) = 0.13819660;  s(4) = 0.13819660;  t(4) = 0.58541020;

    w    = 0.041666666667*ones(4,1);

  elseif (rule == 5)
    % The following points correspond to a 5 point rule
    % Polynomials of degree 3 are integrated exactly
    r    = zeros(5,1);  s    = zeros(5,1);  t    = zeros(5,1);
    r(1) = 0.25000000000;  s(1) = 0.25000000000;  t(1) = 0.25000000000;
    r(2) = 0.16666666666;  s(2) = 0.16666666666;  t(2) = 0.16666666666;
    r(3) = 0.50000000000;  s(3) = 0.16666666666;  t(3) = 0.16666666666;
    r(4) = 0.16666666666;  s(4) = 0.50000000000;  t(4) = 0.16666666666;
    r(5) = 0.16666666666;  s(5) = 0.16666666666;  t(5) = 0.50000000000;

    w    = zeros(5,1);
    w(1) =-0.13333333333;
    w(2) = 0.07500000000;
    w(3) = 0.07500000000;
    w(4) = 0.07500000000;
    w(5) = 0.07500000000;

  else
    error('quadrature rules other than 1, 4 or 5 are not supported\n');
    keyboard
  end


function value = r8_gamr ( x )

%*****************************************************************************80
%
%% R8_GAMR evaluates the reciprocal gamma function of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 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, real X, the argument.
%
%    Output, real VALUE, the value of the reciprocal gamma
%    function at X.
%
  if ( x <= 0.0 && r8_aint ( x ) == x )

    value = 0.0;

  elseif ( abs ( x ) <= 10.0 )

    value = 1.0 / r8_gamma ( x );

  else

    [ alngx, sgngx ] = r8_lgams ( x );
    value = sgngx * exp ( - alngx );

  end

  return
end


function A = group_sum_matrix(G,k)
  % GROUP_SUM_MATRIX Builds a matrix A such that A*V computes the sum of
  % vertices in each group specified by G
  %
  % A = group_sum_matrix(G,k)
  %
  % Inputs:
  %   G  #V list of group indices (1 to k) for each vertex, such that vertex i
  %     is assigned to group G(i)
  %   k  #groups, default is max(G)
  % Outputs:
  %   A  #groups by #V sparse matrix such that A*V = group_sums
  %
  % See also: centroid_matrix

  % number of vertices
  n = numel(G);
  % number of groups
  if ~exist('k','var')
    k = max(G);
  end

  indices =  1:n;

  % builds A such that A(i,j) = 1 where i corresponds to group i and j
  % corresponds to vertex j
  A = sparse(G,indices,1,k,n);

end


function A = spm_clusters(L,n)
% Return the cluster index for a point list
% FORMAT A = spm_clusters(L,n)
% L     - locations [x y x]' {in voxels} ([3 x m] matrix)
% n     - connectivity criterion (see spm_bwlabel) [Default: 18]
%
% A     - cluster index or region number ([1 x m] vector)
%__________________________________________________________________________
%
% spm_clusters characterises a point list of voxel values defined with
% their locations (L) in terms of edge, face and vertex connected
% subsets, returning a list of indices in A, such that the ith location
% belongs to cluster A(i) (using an 18 connectivity scheme).
%__________________________________________________________________________
% Copyright (C) 1994-2012 Wellcome Trust Centre for Neuroimaging

% Jesper Andersson
% $Id: spm_clusters.m 4929 2012-09-17 14:21:01Z guillaume $

if isempty(L), A = []; return; end
if nargin < 2, n = 18; end

% Turn location list to binary 3D volume
%--------------------------------------------------------------------------
dim       = [max(L(1,:)) max(L(2,:)) max(L(3,:))];
vol       = zeros(dim(1),dim(2),dim(3));
indx      = sub2ind(dim,L(1,:)',L(2,:)',L(3,:)');
vol(indx) = 1;

% Label each cluster in 3D volume with its own label using an 18
% connectivity criterion
%--------------------------------------------------------------------------
cci = spm_bwlabel(vol,n);

% Map back to list
%--------------------------------------------------------------------------
A = cci(indx);
A = A(:)';


function a = dif2_llt ( n )

%*****************************************************************************80
%
%% DIF2_LLT returns the Cholesky factor of the DIF2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    a(i,i) = sqrt ( i + 1 ) / sqrt ( i );
  end

  for i = 2 : n
    a(i,i-1) = - sqrt ( i - 1 ) / sqrt ( i );
  end

  return
end


function [ x, seed ] = gompertz_sample ( a, b, seed )

%*****************************************************************************80
%
%% GOMPERTZ_SAMPLE samples the Gompertz PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    1 < A, 0 < B.
%
%    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.
%
  [ cdf, seed ] = r8_uniform_01 ( seed );

  x = gompertz_cdf_inv ( cdf, a, b );

  return
end


function [TKenergy]=TKEO(x)
%
%% Utility function to calculate measures based on the nonlinear energy operator
%
% Function to estimate the TKEO of input data, follows the classical rule
% of Teager and Kaiser
%
% Inputs:  x            -> any time-series signal (vector)
%
% =========================================================================
% Output:  TKenergy     -> (nonlinear) Teager-Kaiser Energy Operator (TKEO)
% =========================================================================
%
% Part of the "Speech Disorders" Toolbox
%
% -----------------------------------------------------------------------
% Useful references:
%
% 1) J. Kaiser: On a simple algorithm to calculate the 'energy' of a
%    signal, Proc. IEEE International Conference on Acoustics, Speech, and
%    Signal Processing (ICASSP '90), pp. 381-384, Albuquerque, NM, USA,
%    April 1990
%
% -----------------------------------------------------------------------
%
% Last modified on 24 August 2014
%
% Copyright (c) Athanasios Tsanas, 2014
%
% ********************************************************************
% If you use this program please cite:
%
% 1) A. Tsanas: "Accurate telemonitoring of Parkinson's disease symptom
%    severity using nonlinear speech signal processing and statistical
%    machine learning", D.Phil. thesis, University of Oxford, 2012
% ********************************************************************
%
% For any question, to report bugs, or just to say this was useful, email
% tsanasthanasis@gmail.com

%% Algorithm computation

% The algorithm is computed using the instantaneous value squared minus the
% previous step value times the next step value:[x_n]^2-[x_(n-1)]*(x_(n+1)]
%
%
% 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.

% 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.
%

data_length=length(x);
TKenergy=zeros(data_length,1);

TKenergy(1)=(x(1))^2; % first sample

for n=2:data_length-1
    TKenergy(n)=(x(n))^2-x(n-1)*x(n+1);
end

%  TKenergy(2:data_length-1) = x(2:data_length-1).^2 - x(1:data_length-2).*x(3:data_length); % alternative, vectorized version

TKenergy(data_length)=(x(data_length))^2; % last sample


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%
%   Unit step sequence

%  u[n]
 n1=-3:-1;
 n2=0:5;
 n=[n1 n2];
 u1=zeros(size(n1));
 u2=ones(size(n2));
 u=[u1 u2];
 stem(n,u)

%  second way
 figure
 n=-3:5
 n0=0;
 u=(n>=n0)
 stem(n,u)

 %u[n-n0]
 figure
 n1=-3:1;
 n2=2:5;
 n=[n1 n2];
 u1=zeros(size(n1));
 u2=ones(size(n2));
 u2=ones(size(n2));
 u=[u1 u2];
 stem(n,u)

%  second way
 figure
 n=-3:5
 n0=2;
 u=((n-n0)>=0)
 stem(n,u)


function ng = ball_grid_count ( n, r, c )

%*****************************************************************************80
%
%% BALL_GRID computes grid points inside a ball.
%
%  Discussion:
%
%    The grid is defined by specifying the radius and center of the ball,
%    and the number of subintervals N into which the horizontal radius
%    should be divided.  Thus, a value of N = 2 will result in 5 points
%    along that horizontal line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of subintervals.
%
%    Input, real R, the radius of the ball.
%
%    Input, real C(3), the coordinates of the center of the ball.
%
%    Output, integer NG, the number of grid points inside the ball.
%
  ng = 0;

  for i = 0 : n
    x = c(1) + r * 2 * i / ( 2 * n + 1 );

    for j = 0 : n
      y = c(2) + r * 2 * j / ( 2 * n + 1 );

      for k = 0 : n

        z = c(3) + r * 2 * k / ( 2 * n + 1 );

        if ( r * r < ( x - c(1) ).^2 ...
                   + ( y - c(2) ).^2 ...
                   + ( z - c(3) ).^2 )
          break
        end
        ng = ng + 1;
        if ( 0 < i )
          ng = ng + 1;
        end
        if ( 0 < j )
          ng = ng + 1;
        end
        if ( 0 < k )
          ng = ng + 1;
        end
        if ( 0 < i && 0 < j )
          ng = ng + 1;
        end
        if ( 0 < i && 0 < k )
          ng = ng + 1;
        end
        if ( 0 < j && 0 < k )
          ng = ng + 1;
        end
        if ( 0 < i && 0 < j && 0 < k )
          ng = ng + 1;
        end
      end
    end
  end

  return
end


function S = randskew(n, N)
% Generates random skew symmetric matrices with normal entries.
%
% function S = randskew(n)
% function S = randskew(n, N)
%
% S is an n-by-n-by-N array where each slice S(:, :, i) for i = 1..N is a
% random skew-symmetric matrix with upper triangular entries distributed
% independently following a normal distribution (Gaussian, zero mean, unit
% variance).
%
% By default, N = 1.
%
% See also: randrot randsym randskewh

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Sept. 25, 2012.
% Contributors:
% Change log:
%       June 19, 2019 (NB):
%           Now handles the case n = 1 properly.

    if nargin < 2
        N = 1;
    end

    if n == 1
        S = zeros(1, 1, N);
        return;
    end

    % Subindices of the (strictly) upper triangular entries of an n-by-n
    % matrix
    [I, J] = find(triu(ones(n), 1));

    K = repmat(1:N, n*(n-1)/2, 1);

    % Indices of the strictly upper triangular entries of all N slices of
    % an n-by-n-by-N matrix
    L = sub2ind([n n N], repmat(I, N, 1), repmat(J, N, 1), K(:));

    % Allocate memory for N random skew matrices of size n-by-n and
    % populate each upper triangular entry with a random number following a
    % normal distribution and copy them with opposite sign on the
    % corresponding lower triangular side.
    S = zeros(n, n, N);
    S(L) = randn(size(L));
    S = S - multitransp(S);

end


function value = box_01_contains_point_nd ( dim_num, p )

%*****************************************************************************80
%
%% BOX_01_CONTAINS_POINT_ND determines if a point is inside a unit box in ND.
%
%  Discussion:
%
%    A unit box is assumed to be a rectangle with sides aligned on coordinate
%    axes.  It can be described by its low and high corner, P1 and P2:
%
%      0 <= P(1:DIM_NUM) <= 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real P(DIM_NUM), the point to be checked.
%
%    Output, logical VALUE, is TRUE if P is inside the box.
%
  value = 0;

  for i = 1 : dim_num
    if ( p(i) < 0.0 | 1.0 < p(i) )
      return
    end
  end

  value = 1;

  return
end


function [mu,P,w,cost]=kMeanspp(z,K,maxIter)
%%KMEANSPP Run the k-means++ algorithm for clustering a set of more than K
%          data points into k clusters. The k-means++ algorithm is the
%          same as the general k-means algorithm, except a different
%          initialization routine is used.
%
%INPUTS: z A zDim X numPoints set of vectors that are to be clustered.
%        K The number of clusters to form. K>=numPoints.
%  maxIter The maximum number of iterations to perform for clustering. If
%          omitted, maxIter=1000;
%
%OUTPUTS: mu A zDim X K set of the cluster means.
%          P A zDim X zDim X K set of sample covariance matrices for each
%            of the k clusters, where mu(:,n) is the mean of the nth
%            cluster.
%          w A KX1 vector of weights such that w(n) is the fraction of the
%            total original points assigned to the nth cluster.
%       cost The cost of the k-means assignment. This is the sum of the
%            squared distances between the points assigned to a cluster and
%            the cluster mean.
%
%The k-means algorithm is a suboptimal algorithm that tries to find a set
%of k-means such that the sum of the squared distances from the points to
%the closest means is minimized. The implementation here uses a better
%initialization the k-means++ initialization from [1], which improves the
%probability that the algorithm will converge to a good solution.
%
%Note that the k-means algorithm used a randomized initialization, so one
%will not always get the same results when the function is run twice on the
%same data.
%
%REFERENCES:
%[1] D. Arthur and S. Vassilvitskii, "k-means++: The advantages of careful
%    seeding," in Proceedings of the Eighteenth Annual ACM-SIAM Symposium
%    on Discrete Algorithms, New Orleans, LA, Jan. 2007, pp. 1027-1035.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3)
   maxIter=1000;
end

%This uses the k-mean++ initalization algorithm on the given data.
%K is the number of clusters to form.

zDim=size(z,1);
numPoints=size(z,2);

mu=zeros(zDim,K);

%Use k-mean++ initialization.
%The first center is chosen randomly.
mu(:,1)=z(:,randi([1;numPoints]));
diff=mu(:,1)*ones(1,numPoints)-z;
nearestMuDist=sum(diff.*diff,1);%The squared distances from this point.

for k=2:K
    %The points are chosen with a probability equal to the ratio of the
    %squared distance to the sum of all distances.
    PMF=nearestMuDist/sum(nearestMuDist);%The PMF from which we will draw.
    CMF=cumsum(PMF);

    %We have to find the first element in the CMF that is greater than a
    %random draw; that will be the next center.
    idx=sum(CMF<rand(1))+1;
    %idx=randi([1;numPoints]);%For the regular k-means.
    %Set the center and update the nesrest center distances.
    mu(:,k)=z(:,idx);
    diff=mu(:,k)*ones(1,numPoints)-z;
    nearestMuDist=min(nearestMuDist,sum(diff.*diff,1));
end

%The k-means refinement.
muPrev=ones(zDim,K)*Inf;
numIter=0;
while(sum(sum(muPrev~=mu))~=0&&numIter<maxIter)
    %Calculate the distances between the points and the current set of
    %means.
    minCosts=ones(1,numPoints)*Inf;
    selClust=zeros(1,numPoints);
    for k=1:K
        diff=bsxfun(@minus,z,mu(:,k));%mu(:,k)*ones(1,numPoints)-z;
        dist=sum(diff.*diff,1);%The squared distances from this center.

        sel=dist<minCosts;
        minCosts(sel)=dist(sel);
        %Assign those points to this cluster if it has the lowest cost.
        selClust(sel)=k;
    end

    %Now, all of the points are assigned to clusters, and we can calculate
    %a new set of means.
    muPrev=mu;
    for k=1:K
        mu(:,k)=mean(z(:,selClust==k),2);
    end
    numIter=numIter+1;
end

%The clustering is done. We will now also return the weights and covariance
%estimates. The weights will be the fraction of the total points assigned
%to a given cluster. The covariance will be the sample covariance.
if(nargout>1)
    w=zeros(K,1);
    P=zeros(zDim,zDim,K);
    for k=1:K
        sel=selClust==k;
        w(k)=sum(sel);

        zSel=z(:,sel);

        diff=bsxfun(@minus,zSel,mu(:,k));
        P(:,:,k)=diff*diff'/w(k);
    end
    w=w/sum(w);

    cost=sum(minCosts)/numPoints;
end
end

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


function [R_ned_2_inert, ned_x, ned_y, ned_z] = computeNedFrame(ut, rVectECI, bodyInfo)
    %Source: https://en.wikipedia.org/wiki/North_east_down

    [phi, lambda, ~, ~, ~, ~, ~, ~, REci2Ecef] = getLatLongAltFromInertialVect(ut, rVectECI, bodyInfo);

    REcef2Ned = [-sin(phi)*cos(lambda), -sin(lambda), -cos(phi)*cos(lambda);
                 -sin(phi)*sin(lambda),  cos(lambda), -cos(phi)*sin(lambda);
                  cos(phi),              0,           -sin(phi)]';

    R_ned_2_inert = (REcef2Ned * REci2Ecef)';
    ned_x = R_ned_2_inert(:,1);
    ned_y = R_ned_2_inert(:,2);
    ned_z = R_ned_2_inert(:,3);
end


function a = stripe ( n )

%*****************************************************************************80
%
%% STRIPE returns the STRIPE matrix.
%
%  Example:
%
%    N = 7
%
%    5  2  1  1  .  .  .
%    2  6  3  1  1  .  .
%    1  3  6  3  1  1  .
%    1  1  3  6  3  1  1
%    .  1  1  3  6  3  1
%    .  .  1  1  3  6  2
%    .  .  .  1  1  2  5
%
%  Properties:
%
%    A is integral, therefore det ( A ) is integral, and
%    det ( A ) * inverse ( A ) is integral.
%
%    A is symmetric: A' = A.
%
%    A is banded, with bandwidth 7.
%
%    A is centrosymmetric: A(I,J) = A(N+1-I,N+1-J).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Joan Westlake,
%    A Handbook of Numerical Matrix Inversion and Solution of
%    Linear Equations,
%    John Wiley, 1968,
%    ISBN13: 978-0471936756,
%    LC: QA263.W47.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

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

           if ( j == i - 3 )
        a(i,j) = 1.0;
      elseif ( j == i - 2 )
        a(i,j) = 1.0;
      elseif ( j == i - 1 )
        if ( j == 1 | j == n - 1 )
          a(i,j) = 2.0;
        else
          a(i,j) = 3.0;
        end
      elseif ( j == i )
        if ( i == 1 | i == n )
          a(i,j) = 5.0;
        else
          a(i,j) = 6.0;
        end
      elseif ( j == i + 1 )
        if ( j == 2 | j == n )
          a(i,j) = 2.0;
        else
          a(i,j) = 3.0;
        end
      elseif ( j == i + 2 )
        a(i,j) = 1.0;
      elseif ( j == i + 3 )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


clear;  clc;
% P2.5
n = 0:10;
x = 10*exp(-j*0.4*pi*n);
[xe, xo, m] = evenoddcomplex(x,n);
subplot(3,2,1); stem(n,real(x),'ko'); title('Real Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(3,2,3); stem(m,real(xe),'ko'); title('Real Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(3,2,5); stem(m,real(xo),'ko'); title('Real Odd Part')
xlabel('n'); ylabel('xo(n)');
subplot(3,2,2); stem(n,imag(x),'ko'); title('Imaginary Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(3,2,4); stem(m,imag(xe),'ko'); title('Imaginary Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(3,2,6); stem(m,imag(xo),'ko'); title('Imaginary Odd Part')
xlabel('n'); ylabel('xo(n)');


function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points_fisheye(X,om,T,f,c,k,alpha)

%project_points2.m
%
%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points_fisheye(X,om,T,f,c,k,alpha)
%
%Projects a 3D structure onto the image plane of a fisheye camera.
%
%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
%       f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)
%       c: principal point location in pixel units (2x1 vector)
%       k: Distortion fisheye coefficients (5x1 vector)
%       alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels)
%
%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)
%        dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)
%        dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)
%        dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar)
%        dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)
%        dxpdk: Derivative of xp with respect to k ((2N)x5 matrix)
%
%Definitions:
%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
%The coordinate vector of P in the camera reference frame is: Xc = R*X + T
%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
%call r^2 = a^2 + b^2,
%call theta = atan(r),
%Fisheye distortion -> theta_d = theta * (1 + k(1)*theta^2 + k(2)*theta^4 + k(3)*theta^6 + k(4)*theta^8)
%
%The distorted point coordinates are: xd = [xx;yy] where:
%
%xx = (theta_d / r) * x
%yy = (theta_d / r) * y
%
%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
%
%xxp = f(1)*(xx + alpha*yy) + c(1)
%yyp = f(2)*yy + c(2)
%
%
%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices
%
%
%Important function called within that program:
%
%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
%
%rigid_motion.m: Computes the rigid motion transformation of a given structure

if nargin < 7,
    alpha = 0;
    if nargin < 6,
        k = zeros(4,1);
        if nargin < 5,
            c = zeros(2,1);
            if nargin < 4,
                f = ones(2,1);
                if nargin < 3,
                    T = zeros(3,1);
                    if nargin < 2,
                        om = zeros(3,1);
                        if nargin < 1,
                            error('Need at least a 3D structure to project (in project_points.m)');
                            return;
                        end;
                    end;
                end;
            end;
        end;
    end;
end;

[m,n] = size(X);

if nargout > 1,
    [Y,dYdom,dYdT] = rigid_motion(X,om,T);
else
    Y = rigid_motion(X,om,T);
end;

inv_Z = 1./Y(3,:);

x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;

bb = (-x(1,:) .* inv_Z)'*ones(1,3);
cc = (-x(2,:) .* inv_Z)'*ones(1,3);

if nargout > 1,
    dxdom = zeros(2*n,3);
    dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);
    dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);

    dxdT = zeros(2*n,3);
    dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);
    dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);
end;

% Add fisheye distortion:

r2 = x(1,:).^2 + x(2,:).^2;

if nargout > 1,
    dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);
    dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);
end;

% Radial distance:
r = sqrt(r2);
if nargout > 1,
    drdr2 = ones(1,length(r));
    drdr2(r>1e-8) = 1 ./ (2*r(r>1e-8));

    drdom = [ (drdr2').*dr2dom(:,1)   (drdr2').*dr2dom(:,2)  (drdr2').*dr2dom(:,3)  ];
    drdT = [ (drdr2').*dr2dT(:,1)   (drdr2').*dr2dT(:,2)  (drdr2').*dr2dT(:,3)  ];
end;

% Angle of the incoming ray:
theta = atan(r);
if nargout > 1,
    dthetadr = 1 ./ (1 + r2);

    dthetadom = [ (dthetadr').*drdom(:,1)   (dthetadr').*drdom(:,2)  (dthetadr').*drdom(:,3) ];
    dthetadT = [ (dthetadr').*drdT(:,1)   (dthetadr').*drdT(:,2)  (dthetadr').*drdT(:,3) ];
end;

% Add the fisheye distortion:

theta2 = theta.^2;
theta3 = theta2.*theta;
theta4 = theta2.^2;
theta5 = theta4.*theta;
theta6 = theta3.^2;
theta7 = theta6.*theta;
theta8 = theta4.*theta4;
theta9 = theta8.*theta;

% Fisheye distortion -> theta_d = theta * (1 + k(1)*theta2 + k(2)*theta4 + k(3)*theta6 + k(4)*theta8)

theta_d = theta + k(1)*theta3 + k(2)*theta5 + k(3)*theta7 + k(4)*theta9;

if nargout > 1,
    dtheta_ddtheta = 1 + 3*k(1)*theta2 + 5*k(2)*theta4 + 7*k(3)*theta6 + 9*k(4)*theta8;
    dtheta_ddom = [ (dtheta_ddtheta').*dthetadom(:,1)   (dtheta_ddtheta').*dthetadom(:,2)  (dtheta_ddtheta').*dthetadom(:,3) ];
    dtheta_ddT = [ (dtheta_ddtheta').*dthetadT(:,1)   (dtheta_ddtheta').*dthetadT(:,2)  (dtheta_ddtheta').*dthetadT(:,3) ];
    dtheta_ddk = [theta3' theta5' theta7' theta9'];
end;

% ratio:
inv_r = ones(1,length(r));
inv_r(r>1e-8) = 1./r(r>1e-8);

cdist = ones(1,length(r));
cdist(r > 1e-8) = theta_d(r > 1e-8) ./ r(r > 1e-8);

if nargout > 1,
    dcdistdom = [ ((inv_r').*(dtheta_ddom(:,1) - (cdist').*drdom(:,1)))  ((inv_r').*(dtheta_ddom(:,2) - (cdist').*drdom(:,2)))   ((inv_r').*(dtheta_ddom(:,3) - (cdist').*drdom(:,3))) ];
    dcdistdT = [ ((inv_r').*(dtheta_ddT(:,1) - (cdist').*drdT(:,1)))  ((inv_r').*(dtheta_ddT(:,2) - (cdist').*drdT(:,2)))   ((inv_r').*(dtheta_ddT(:,3) - (cdist').*drdT(:,3))) ];
    dcdistdk = [ (inv_r'.*dtheta_ddk(:,1)) (inv_r'.*dtheta_ddk(:,2)) (inv_r'.*dtheta_ddk(:,3))  (inv_r'.*dtheta_ddk(:,4)) ];
end;

xd1 = x .* (ones(2,1)*cdist);

if nargout > 1,
    dxd1dom = zeros(2*n,3);
    dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;
    dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;
    coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
    dxd1dom = dxd1dom + coeff.* dxdom;

    dxd1dT = zeros(2*n,3);
    dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;
    dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;
    dxd1dT = dxd1dT + coeff.* dxdT;

    dxd1dk = zeros(2*n,4);
    dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk;
    dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk;
end;

% No tangential distortion:
xd2 = xd1;
if nargout > 1,
    dxd2dom = dxd1dom;
    dxd2dT = dxd1dT;
    dxd2dk = dxd1dk;
end;

% Add Skew:
xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)];

% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha
if nargout > 1,
    dxd3dom = zeros(2*n,3);
    dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:);
    dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:);
    dxd3dT = zeros(2*n,3);
    dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:);
    dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:);
    dxd3dk = zeros(2*n,4);
    dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:);
    dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:);
    dxd3dalpha = zeros(2*n,1);
    dxd3dalpha(1:2:2*n,:) = xd2(2,:)';
end;

% Pixel coordinates:
if length(f)>1,
    xp = xd3 .* (f * ones(1,n))  +  c*ones(1,n);
    if nargout > 1,
        coeff = reshape(f*ones(1,n),2*n,1);
        dxpdom = (coeff*ones(1,3)) .* dxd3dom;
        dxpdT = (coeff*ones(1,3)) .* dxd3dT;
        dxpdk = (coeff*ones(1,4)) .* dxd3dk;
        dxpdalpha = (coeff) .* dxd3dalpha;
        dxpdf = zeros(2*n,2);
        dxpdf(1:2:end,1) = xd3(1,:)';
        dxpdf(2:2:end,2) = xd3(2,:)';
    end;
else
    xp = f * xd3 + c*ones(1,n);
    if nargout > 1,
        dxpdom = f  * dxd3dom;
        dxpdT = f * dxd3dT;
        dxpdk = f  * dxd3dk;
        dxpdalpha = f .* dxd3dalpha;
        dxpdf = xd3(:);
    end;
end;

if nargout > 1,
    dxpdc = zeros(2*n,2);
    dxpdc(1:2:end,1) = ones(n,1);
    dxpdc(2:2:end,2) = ones(n,1);
end;

return;

% Test of the Jacobians:

n = 10;

X = 10*randn(3,n);
om = randn(3,1);
T = [10*randn(2,1);40];
f = 1000*rand(2,1);
c = 1000*randn(2,1);
k = 0.5*randn(4,1);
alpha = 0.01*randn(1,1);

[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X,om,T,f,c,k,alpha);

% Test on om: not OK

dom = 0.00000000001 * norm(om)*randn(3,1);
om2 = om + dom;

[x2] = project_points_fisheye(X,om2,T,f,c,k,alpha);

x_pred = x + reshape(dxdom * dom,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on T: not OK

dT = 0.0001 * norm(T)*randn(3,1);
T2 = T + dT;

[x2] = project_points_fisheye(X,om,T2,f,c,k,alpha);

x_pred = x + reshape(dxdT * dT,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on f: OK!!

df = 0.001 * norm(f)*randn(2,1);
f2 = f + df;

[x2] = project_points_fisheye(X,om,T,f2,c,k,alpha);

x_pred = x + reshape(dxdf * df,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on c: OK!!

dc = 0.01 * norm(c)*randn(2,1);
c2 = c + dc;

[x2] = project_points_fisheye(X,om,T,f,c2,k,alpha);

x_pred = x + reshape(dxdc * dc,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on k: OK!!

dk = 0.00001 * norm(k)*randn(4,1);
k2 = k + dk;

[x2] = project_points_fisheye(X,om,T,f,c,k2,alpha);

x_pred = x + reshape(dxdk * dk,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on alpha: OK!!

dalpha = 0.001 * norm(k)*randn(1,1);
alpha2 = alpha + dalpha;

[x2] = project_points_fisheye(X,om,T,f,c,k,alpha2);

x_pred = x + reshape(dxdalpha * dalpha,2,n);

norm(x2-x)/norm(x2 - x_pred)


function [phi, theta, psi] = rotMatXYZ2Eul(rotMat)
    % Function to convert rotation matrix to Euler angles (extrinsic x-y-z convention).
    % Used in yaw to convert between global and local reference frames.
    %
    % Parameters
    % ------------
    %     rotMat : float array [3,3]
    %         Rotational matrix to transform vectors to the new frame
    %
    % Returns
    % ------------
    %     phi : float [rad]
    %         Angle of rotation about the x-axis
    %     theta : float [rad]
    %         Angle of rotation about the y-axis
    %     psi : float [rad]
    %         Angle of rotation about the z-axis

    phi = atan2(-rotMat(2,3), rotMat(3,3));
    theta = asin(rotMat(1,3));
    psi = atan2(-rotMat(1,2), rotMat(1,1));
end


%% Equation: Poisson Equation Discretized by $BDM_1$ Element in 2D
% We explain the assembling of the matrix equation for the lowest order BDM element
% discretization of Poisson equation.
%
% [u,sigma] = PoissonBDM1(node,elem,bdEdge,f,g_D,varargin)
%
% Created by Ming Wang at Jan 2. 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Data Structure
%   [elem2dof,dofSign,edge] = dofRT0(elem);
%
% will construct local to global index map; see ifem dofBDM1doc for
% details.
%
%% Local Bases
% Suppose [i,j] is the k-th edge. The hierarchic basis along with
% their div are given by
%
% $$ \phi_k = \lambda_i rot \lambda_j - \lambda_j rot \lambda_i,\quad
%    \psi_k = \lambda_i rot \lambda_j + \lambda_j rot \lambda_i. $$
%
% $$ div \phi_k = 2 \nabla \lambda_i \cdot rot \lambda_j, \quad div \psi_k = 0.$$
%
% Inside one triangular, the 6 bases functions along with their div
% corresponding to 3 local edges [2 3; 3 1; 1 2] are:
%
% $$ \phi_1 = \lambda_2 rot \lambda_3 - \lambda_3 rot \lambda_2,\quad
%    \psi_1 = \lambda_2 rot \lambda_3 + \lambda_3 rot \lambda_2,\quad
%    div\phi_1 = 2 \nabla \lambda_2 \cdot rot \lambda_3, \quad div \psi_1 = 0. $$
%
% $$ \phi_2 = \lambda_3 rot \lambda_1 - \lambda_1 rot \lambda_3,\quad
%    \psi_2 = \lambda_3 rot \lambda_1 + \lambda_1 rot \lambda_3,\quad
%    div\phi_2 = 2 \nabla \lambda_3 \cdot rot \lambda_1, \quad div \psi_2 = 0. $$
%
% $$ \phi_3 = \lambda_1 rot \lambda_2 - \lambda_2 rot \lambda_1,\quad
%    \psi_3 = \lambda_1 rot \lambda_2 + \lambda_2 rot \lambda_1,\quad
%    div\phi_3 = 2 \nabla \lambda_1 \cdot rot \lambda_2, \quad div \psi_3 = 0. $$
%
% Locally, we order the local bases in the following way:
%
% $$\{\phi_1,~\,\phi_2,~\,\phi_3,~\,\psi_1,~\,\psi_2,~\, \psi_3.\}$$
%
% Note that $RT_0 \subset BDM_1$, and $\{ \phi_1,~\,\phi_2,~\,\phi_3 \}$ is the
% local bases for $RT_0$.
%
% Because of the different oritentation of local and global faces, from
% local bases to the global one, the direction should be corrected. That is
%
%    phiGlobal(elem2dof(t,1),:) = phi(t,1)*dofSign(t,1);

%% Mass Matrix
% We use the integral formula
%
% $$ \int_T
% \lambda_1^{\alpha_1}\lambda_2^{\alpha_2}\lambda_3^{\alpha_3}
% dx = \frac{\alpha_1!\alpha_2!\alpha_3!2!}{(\sum _{i=1}^3\alpha_i
% + 2)!}\;|T|,$$
%
% to get
%
% $$ \int _T\lambda_i\lambda_j dx = (1+(i==j))|T|/12. $$
%
% For two local bases $\phi_i$ and $\phi_j$ corresponding to the ith and
% jth local edges, suppose i = [i1 i2], j = [j1 j2].
% $$\int_T \phi_i \phi_j dx = \int_T (
%  \lambda_{i1} rot \lambda_{i2}\cdot\lambda_{j1} rot \lambda_{j2}
% -\lambda_{i2} rot \lambda_{i1}\cdot\lambda_{j1} rot \lambda_{j2}
% -\lambda_{i1} rot \lambda_{i2}\cdot\lambda_{j2} rot \lambda_{j1}
% +\lambda_{i2} rot \lambda_{i1}\cdot\lambda_{j2} rot \lambda_{j1})dx
% $$
%
% $\int_T \psi_i \psi_j dx$ and $\int_T \psi_i \phi_j dx$ can be computed similarly.
%
%% Matrix for Differential Operator
% Note that $\nabla \cdot \psi = 0$, We only need to record $\nabla \cdot \phi_i$ and
% then the computation $\int _T \nabla
% \cdot \phi_i dx$ is straightforward. Just remember to correct the direction.

%% Right hand side
% We use 5-points quadrature which is exact for cubic polynomials. In the
% barycentric coordinate, the 5-points are
%
% $$ p_1 = [2/3, 1/6, 1/6], \quad  w_1 = 1/3 $$
%
% $$ p_2 = [1/6, 2/3, 1/6], \quad  w_2 = 1/3 $$
%
% $$ p_3 = [1/6, 1/6, 2/3], \quad  w_3 = 1/3 $$
%
%%
% Note that the two for loops are nested in such a way that the point pxy
% and the evulation Jp is just computed once.
%
% The local to global assembling is computed using accumarray
%
%   b = accumarray(elem2dof(:),bt(:),[Ndof 1]);


function [v,x,t,m,ze]=quadpeak(z)
%PEAK2DQUAD find quadratically-interpolated peak in a N-D array
%
%  Inputs:  Z(m,n,...)   is the input array (ignoring trailing singleton dimensions)
%                        Note: a row vector will have 2 dimensions
%
% Outputs:  V        is the peak value
%           X(:,1)  is the position of the peak (in fractional subscript values)
%           T        is -1, 0, +1 for maximum, saddle point or minimum
%           M        defines the fitted quadratic: z = [x y ... 1]*M*[x y ... 1]'
%           ZE       the estimated version of Z

%	   Copyright (C) Mike Brookes 2008
%      Version: $Id: quadpeak.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent wz a
% first calculate the fixed matrix, a (can be stored if sz is constant)
sz=size(z);         % size of input array
psz=prod(sz);       % number of elements in input array
dz=numel(sz);       % number of input dimensions
mz=find(sz>1);      % non-singleton dimension indices
nm=numel(mz);       % number of non-singleton dimensions
vz=sz(mz);          % size of squeezed input array
dx=max(mz);         % number of output dimensions
if ~nm              % if the input array is a scalar
    error('Cannot find peak of a scalar');
end
nc=(nm+1)*(nm+2)/2;  % number of columns in A matrix
if min(vz)<3
    error('Need at least 3 points in each non-singleton dimension');
end
if isempty(wz) || numel(wz)~=numel(vz) || ~all(wz==vz)
    wz=vz;
    a=ones(psz,nc);
    ix=(0:psz-1)';
    for i=1:nm
        jx=floor(ix/sz(mz(i)));
        a(:,i+nc-nm-1)=1+ix-jx*sz(mz(i));
        ix=jx;
        a(:,(i^2-i+2)/2:i*(i+1)/2)=a(:,nc-nm:i+nc-nm-1).*repmat(a(:,i+nc-nm-1),1,i);
    end
    a=(a'*a)\a';        % converts to polynomial coeficients {x^2 xy y^2 x y 1]
end

% now find the peak

c=a*z(:);   % polynomial coefficients for this data
w=zeros(nm+1,nm+1);
i=1:(nm+1)*(nm+2)/2;
j=floor((sqrt(8*i-7)-1)/2);
w(i+j.*(2*nm+1-j)/2)=c;
w=(w+w.')/2; % make it symmetrical
mr=w(1:nm,1:nm);
we=w(1:nm,nm+1);
y=-(mr\we);
v=y'*we+w(nm+1,nm+1);  % value at peak

% insert singleton dimensions into outputs

x=zeros(dx,1);
x(mz)=y;
m=zeros(dx+1,dx+1);
mz(nm+1)=dx+1;
m(mz,mz)=w;
if nargout>2
    ev=eig(mr);
    t=all(ev>0)-all(ev<0);
end
if nargout>4
    ze=zeros(sz);
    scp=cumprod([1 sz(1:end-1)]);
    ivec=fix(repmat((0:psz-1)',1,dz)./repmat(scp,psz,1));
    xe=[1+ivec-repmat(sz,psz,1).*fix(ivec./repmat(sz,psz,1)) ones(psz,1)];
    ze=reshape(sum((xe*m).*xe,2),sz);
end

if ~nargout && nm<=2
    % plot the data
    desc={'Maximum','Saddle Point','Minimum'};
    if nargout<=2
        ev=eig(mr);
        t=all(ev>0)-all(ev<0);
    end
    if nm==1
        xax=linspace(1,psz,100);
        plot(xax,c(1)*xax.^2+c(2)*xax+c(3),'-r',1:psz,z(:),'ob',x,v,'^k');
        set(gca,'xlim',[0.9 psz+0.1]);
        ylabel('z');
        xlabel(sprintf('x%d',mz(1)));
        title(sprintf('\\Delta = %s: z(%.2g) = %.2g',desc{t+2},y(1),v));
    else
        ngr=17;
        xax=repmat(linspace(1,vz(1),ngr)',1,ngr);
        yax=repmat(linspace(1,vz(2),ngr),ngr,1);
        zq=(c(1)*xax+c(2)*yax+c(4)).*xax+(c(3)*yax+c(5)).*yax+c(6);
        hold off
        mesh(xax,yax,zq,'EdgeColor','r');
        hold on
        plot3(repmat((1:vz(1))',1,vz(2)),repmat(1:vz(2),vz(1),1),reshape(z,vz),'ob',y(1),y(2),v,'^k');
        hold off
        set(gca,'xlim',[0.9 vz(1)+0.1],'ylim',[0.9 vz(2)+0.1]);
        xlabel(sprintf('x%d',mz(1)));
        ylabel(sprintf('x%d',mz(2)));
        zlabel('z');
        title(sprintf('\\Delta = %s: z(%.2g,%.2g) = %.2g',desc{t+2},y(1),y(2),v));
    end
end


function [x C d] = project_affine(v, A, b, C, d)
% PROJECT_AFFINE    Project a point into an affine set.
%
%   project_affine(v,A,b) is the projection of v onto
%   the affine set { x | Ax = b }.
%
%   You can also call the function as
%
%     [x C d] = project_affine(v,A,b);
%
%   and then call it again with different argument v2 via
%
%     x2 = project_affine(v2,A,b,C,d);
%
%   If calling the function repeatedly with the same A and b,
%   all evaluations after the initial one will be much faster
%   when passing in the cached values C and d.

    if exist('C','var') && ~isempty(C) && exist('d','var') && ~isempty(d)
        x = C*v + d;
    else
        % x = v - pinv(A)*(A*v - b);
        pA = pinv(A);
        C = eye(length(v)) - pA*A;
        d = pA*b;
        x = C*v + d;
    end
end


function x_opt = coordinate_search ( x, function_handle, flag )

%*****************************************************************************80
%
%% COORDINATE_SEARCH carries out a direct search minimization algorithm.
%
%  Discussion:
%
%    This function implements a direct search algorithm, to seek a
%    multidimensional point X_OPT which minimizes a scalar function F(X).
%
%    The function is currently set up for coordinate search by
%    using a default stencil along coordinate axes.  The stencil and the
%    initial size of the stencil (delta) should probably be input parameters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2009
%
%  Author:
%
%    Jeff Borggaard
%
%  Parameters:
%
%    Input, real X(N_VAR), a starting estimate for the minimizer.
%
%    Input, function handle FUNCTION_HANDLE ( X ), ...
%
%    Optional input, integer FLAG, selects features of the algorithm.
%    If FLAG is not input, it defaults to the value 0.
%    0, do not include graphical displays.
%    nonzero, display the status of the search graphically.
%    3, use the water stick stencil.
%
%    Output, real X_OPT(N_VAR), the estimated minimizer.
%
%  Local Parameters:
%
%    Local, real DELTA, the initial size of the stencil.
%
%    Local, real TOLERANCE, a tolerance for the variation between the maximum
%    and minimum values of F in the stencil.
%
%    Local, integer MAX_FEVAL, the maximum number of function evaluations.
%
  delta = 1.0;
  tolerance = 1.0E-06;
  max_feval = 250;
  verbose = 0;
%
%  The default value of FLAG is 0.
%
  if ( nargin <= 2 )
    flag = 0;
  end

  x = x(:)';
  n_var = length(x);
%
%  Define the search stencil
%
  if ( flag ~= 3 )

    for i = 1 : n_var
      stencil(2*i-1).v    = zeros(size(x));
      stencil(2*i-1).v(i) = 1;
      stencil(2*i  ).v    = zeros(size(x));
      stencil(2*i  ).v(i) =-1;
    end

  else

    stencil(1).v = [   1        0    ];
    stencil(2).v = [ -1/2  sqrt(3)/2 ];
    stencil(3).v = [ -1/2 -sqrt(3)/2 ];

  end

  n_sten = length ( stencil );
%
%  Compute the function value at the initial point.
%
  f = feval ( function_handle, x );
  n_feval = 1;

  if ( flag )
    xp = linspace ( -5.0, 5.0, 101 );
    yp = xp;
    for i=1:101
      for j=1:101
        fp(j,i) = feval(function_handle,[xp(i),yp(j)]);
      end
    end

    figure(27)
    hold on
    contour(xp,yp,fp,linspace(0,200,25))
  end
%
%  Begin the Direct Search iteration.
%
  while ( 1 )

    x_s = compute_points ( x, stencil, delta );
%
%  Plot the current search pattern.
%
    if ( flag )
      for i=1:n_sten
        plot([x(1) x_s(i,1)],[x(2) x_s(i,2)],'r')
        plot(x_s(i,1),x_s(i,2),'r*')
      end
    end

    f_s = evaluate ( x_s, function_handle );
    n_feval = n_feval + n_sten;
%
% "erase" the current search pattern after it has been considered
%
    if ( flag )

      for i = 1 : n_sten
        plot ( [x(1) x_s(i,1)],[x(2) x_s(i,2)],'b')
        plot ( x_s(i,1),x_s(i,2),'b*')
      end
    end
%
%  Find the element in the pattern with the lowest function value.
%
    [ fc_min, i_min ] = min ( f_s );
    [ fc_max        ] = max ( f_s );
%
%  If we found a lower value, move to that location.
%
    if ( fc_min < f )

      xb = x_s(i_min,:);
      shift = xb - x;

      x = xb;
      f = fc_min;

      for i = 1 : n_sten
        x_s(i,:) = x_s(i,:) + shift;
      end
%
%  Otherwise, shrink the pattern about the center and recalculate the stencil.
%
    else
      delta = delta / 2.0;
      x_s = compute_points ( x, stencil, delta );
    end
%
%  Test whether we should continue.
%
    converged = ( fc_max - fc_min < tolerance ) || ( delta < tolerance );

    if ( converged )
      break
    end

    diverged = ( max_feval < n_feval );

    if ( diverged )
      break
    end

  end

  if ( verbose )
    fprintf ( 'The best point x^* was: %d %d\n', x(1,:) );
    fprintf ( 'f(x^*) = %d\n', f(1) );
  end

  x_opt = x(1,:);

  if ( verbose )
    fprintf ( 'The algorithm terminated after %d function evaluations\n',...
      n_feval);
  end

  if ( diverged )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Convergence was not achieved.\n' );
    fprintf ( 1, '  The iteration limit of %d was exceeded.\n', max_feval );
  end

  return
end
function f = evaluate ( x, function_handle )

%*****************************************************************************80
%
%% EVALUATE evaluates the function at the stencil points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2009
%
%  Author:
%
%    Jeff Borggaard
%
%  Parameters:
%
%    Input, real X(N_STEN,N_VAR), a set of N_STEN points, each of
%    dimension N_VAR.
%
%    Input, f = FUNCTION_HANDLE ( x ), the handle of a function which
%    accepts as input a vector X and returns the scalar function value F.
%
%    Output, real F(N_STEN), the function evaluated at each of the points.
%
  [ n_sten, n_var ] = size ( x );

  f = zeros ( 1, n_sten );

  for i = 1 : n_sten
    f(i) = feval ( function_handle, x(i,:) );
  end

  return
end
function x_s = compute_points ( x, stencil, delta )

%*****************************************************************************80
%
%% COMPUTE_POINTS computes all of the points in the stencil.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2009
%
%  Author:
%
%    Jeff Borggaard
%
%  Parameters:
%
%    Input, real X(N_VAR), the center of the stencil.
%
%    Input, real STENCIL, a structure describing the stencil.
%
%    Input, real DELTA, determines the geometric size of the stencil.
%
%    Output, real X_S(N_STEN,N_VAR), the stencil points.
%
  n_var = length(x);
  n_sten = length(stencil);

  x_s = zeros ( n_sten, n_var );

  for i = 1 : n_sten
    x_s(i,:) = x + delta * stencil(i).v;
  end

  return
end


function pts = cuboid_prm2pts(prm, vp)
% converts five parameters of cuboid to eight 2D points
%
% prm: [x, y, w, h, d]
% x,y: 2D coordinates of corner1 (top,left,front corner)
% w: length (in pixels) from corner1 to corner2
% h: length (in pixels) from corner1 to corner3
% d: length (in pixels) from corner1 to corner5
%
% pts: [8 x 2] 2D coordinates of eight points on cuboid
%   5---6   1------2
%  /   /|   |      |
% 1---2 8   3------4    ....
% |   |/     \    /
% 3---4       7--8
%
% 1: front,top,left
% 2: front,top,right
% ...
% 8: back,bottom,right

pts = zeros(8,2);

pts(1,:) = [prm(1) prm(2)];
pts(2,:) = towardsright(pts(1,:), vp, prm(3));
pts(3,:) = towardsdown(pts(1,:), vp, prm(4));
pts(5,:) = towardsback(pts(1,:), vp, prm(5));
pts(4,:) = line_intersect(pts(2,:), vp{1}, pts(3,:), vp{2});
pts(6,:) = line_intersect(pts(2,:), vp{3}, pts(5,:), vp{2});
pts(7,:) = line_intersect(pts(3,:), vp{3}, pts(5,:), vp{1});
pts(8,:) = mean(...
    [line_intersect(pts(4,:), vp{3}, pts(6,:), vp{1}); ...
     line_intersect(pts(7,:), vp{2}, pts(6,:), vp{1}); ...
     line_intersect(pts(4,:), vp{3}, pts(7,:), vp{2})], 1);


function r8sd_print ( n, ndiag, offset, a, title )

%*****************************************************************************80
%
%% R8SD_PRINT prints a R8SD matrix.
%
%  Discussion:
%
%    The R8SD storage format is for symmetric matrices whose only nonzero entries
%    occur along a few diagonals, but for which these diagonals are not all
%    close enough to the main diagonal for band storage to be efficient.
%
%    In that case, we assign the main diagonal the offset value 0, and
%    each successive superdiagonal gets an offset value 1 higher, until
%    the highest superdiagonal (the A(1,N) entry) is assigned the offset N-1.
%
%    Assuming there are NDIAG nonzero diagonals (ignoring subdiagonals!),
%    we then create an array B that has N rows and NDIAG columns, and simply
%    "collapse" the matrix A to the left:
%
%  Example:
%
%    The "offset" value is printed above each column.
%
%    Original matrix               New Matrix
%
%       0   1   2   3   4   5       0   1   3   5
%
%      11  12   0  14   0  16      11  12  14  16
%      21  22  23   0  25   0      22  23  25  --
%       0  32  33  34   0  36      33  34  36  --
%      41   0  43  44  45   0      44  45  --  --
%       0  52   0  54  55  56      55  56  --  --
%      61   0  63   0  65  66      66  --  --  --
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of columns of the matrix.
%    N must be positive.
%
%    Input, integer NDIAG, the number of diagonals of the matrix
%    that are stored in the array.
%    NDIAG must be at least 1, and no more than N.
%
%    Input, integer OFFSET(NDIAG), the offsets for the diagonal storage.
%
%    Input, real A(N,NDIAG), the R8SD matrix.
%
%    Input, string TITLE, a title to be printed.
%
  r8sd_print_some ( n, ndiag, offset, a, 1, 1, n, n, title );

  return
end


function [U S V info] = truncated_svd(A, p)
% Returns an SVD decomposition of A truncated to rank p.
%
% function [U S V] = truncated_svd(A, p)
%
% Input: A real matrix A of size mxn and an integer p <= min(m, n).
% Output: An orthonormal matrix U of size mxp, an orthonormal matrix Y of
%         size nxp and a diagonal matrix S of size pxp with nonnegative and
%         decreasing diagonal entries such that USV.' is the best rank p
%         approximation of A according to the Frobenius norm. All real.
%         This function produces an output akin to svds.
%
% The decomposition is obtained by maximizing
%   f(U, V) = .5*norm(U'*A*V, 'fro')^2
% where U, V are orthonormal. Notice that f(U*Q, V*R) = f(U, V) for all
% Q, R orthogonal pxp matrices. Hence, only the column spaces of U and V
% matter and we may perform the optimization over a product of two
% Grassmannian manifolds.
%
% It is easy to show that maximizing f is equivalent to minimizing g with
%   g(U, V) = min_S norm(U*S*V' - A, 'fro')^2,
% which confirms that we are going for a best low-rank approximation of A.
%
% The inner workings of the Grassmann manifold use the built-in svd
% function of Matlab but only for matrices of size mxp and nxp to
% re-orthonormalize them.
%
% Notice that we are actually chasing a best fixed-rank approximation of a
% matrix, which is best obtained by working directly over a manifold of
% fixed-rank matrices. This is simply an example script to demonstrate some
% functionalities of the toolbox.
%
% The code can be modified to accept a function handle for A(x) = A*x
% instead of a matrix A, which is often useful. This would further require
% a function handle At for the transpose of A, such that At(x) = A.'*x.

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, July 5, 2013
% Contributors:
%
% Change log:
%

    % Generate some random data to test the function if none is given.
    if ~exist('A', 'var') || isempty(A)
        A = randn(42, 60);
    end
    if ~exist('p', 'var') || isempty(p)
        p = 5;
    end

    % Retrieve the size of the problem and make sure the requested
    % approximation rank is at most the maximum possible rank.
    [m n] = size(A);
    assert(p <= min(m, n), 'p must be smaller than the smallest dimension of A.');

    % Define the cost and its derivatives on the Grassmann manifold
    tuple.U = grassmannfactory(m, p);
    tuple.V = grassmannfactory(n, p);
    % All of the code will work just as well if we ignore the invariance
    % property of the cost function indicated above and thus place U and V
    % on the Stiefel manifold (orthonormal matrices) instead of the
    % Grassmann manifold. Working on Stiefel is expected to be slower
    % though, partly because de search space is higher dimensional and
    % partly because the optimizers are not isolated.
    % tuple.U = stiefelfactory(m, p);
    % tuple.V = stiefelfactory(n, p);
    M = productmanifold(tuple);

    % Define a problem structure, specifying the manifold M, the cost
    % function and its derivatives. Here, to demonstrate the rapid
    % prototyping capabilities of Manopt, we directly define the Euclidean
    % gradient and the Euclidean Hessian egrad and ehess instead of the
    % Riemannian gradient and Hessian grad and hess. Manopt will take care
    % of the conversion. This automatic conversion is usually not
    % computationally optimal though, because much of the computations
    % involved in obtaining the gradient could be reused to obtain the
    % Hessian. After the prototyping stage, when efficiency becomes
    % important, it makes sense to define grad and hess rather than egrad
    % an ehess, and to use the caching system (the store structure).
    problem.M = M;
    problem.cost  = @cost;
    problem.egrad = @egrad;
    problem.ehess = @ehess;

    % The functions below make many redundant computations. This
    % performance hit can be alleviated by using the caching system.

    % Cost function
    function f = cost(X)
        U = X.U;
        V = X.V;
        f = -.5*norm(U'*A*V, 'fro')^2;
    end
    % Euclidean gradient of the cost function
    function g = egrad(X)
        U = X.U;
        V = X.V;
        AV = A*V;
        AtU = A'*U;
        g.U = -AV*(AV'*U);
        g.V = -AtU*(AtU'*V);
    end
    % Euclidean Hessian of the cost function
    function h = ehess(X, H)
        U = X.U;
        V = X.V;
        Udot = H.U;
        Vdot = H.V;
        AV = A*V;
        AtU = A'*U;
        AVdot = A*Vdot;
        AtUdot = A'*Udot;
        h.U = -(AVdot*AV'*U + AV*AVdot'*U + AV*AV'*Udot);
        h.V = -(AtUdot*AtU'*V + AtU*AtUdot'*V + AtU*AtU'*Vdot);
    end

    % Execute some checks on the derivatives for early debugging.
    % These things can be commented out of course.
    checkgradient(problem);
    pause;
    checkhessian(problem);
    pause;

    % Issue a call to a solver. A random initial guess will be chosen and
    % default options are selected. Here, we specify a maximum trust
    % region radius (which in turn induces an initial trust region radius).
    % Note that this is not required: default values are used if we omit
    % this. The diameter of the manifold scales like sqrt(2*p), hence the
    % form of our (empirical) choice.
    options.Delta_bar = 4*sqrt(2*p);
    [X Xcost info] = trustregions(problem, [], options); %#ok<ASGLU>
    U = X.U;
    V = X.V;

    % Finish the job by rotating U and V such that the middle matrix S can
    % be diagonal with nonnegative, decreasing entries. This requires a
    % small svd of size pxp.
    Spp = U'*A*V;
    [Upp Spp Vpp] = svd(Spp);
    U = U*Upp;
    S = Spp;
    V = V*Vpp;

    % For our information, Manopt can also compute the spectrum of the
    % Riemannian Hessian on the tangent space at (any) X. Computing the
    % spectrum at the solution gives us some idea of the conditioning of
    % the problem. If we were to implement a preconditioner for the
    % Hessian, this would also inform us on its performance.
    %
    % Notice that if the optimization is performed on a product of Stiefel
    % manifolds instead of a product of Grassmannians, the double
    % invariance under the orthogonal group O(p) will appear as twice
    % p*(p-1)/2, thus p*(p-1) zero eigenvalues in the spectrum of the
    % Hessian. This means that the minimizers are not isolated, which
    % typically hinders convergence of second order algorithms.
    if M.dim() < 512
        evs = hessianspectrum(problem, X);
        stairs(sort(evs));
        title(['Eigenvalues of the Hessian of the cost function ' ...
               'at the solution']);
    end

end


function circle_imp_print_2d ( r, center, title )

%*****************************************************************************80
%
%% CIRCLE_IMP_PRINT_2D prints an implicit circle in 2D.
%
%  Discussion:
%
%    An implicit circle in 2D satisfies:
%
%      ( X - CENTER(1) )^2 + ( Y - CENTER(2) )^2 = R^2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the circle.
%
%    Input, real CENTER(2,1), the center of the circle.
%
%    Input, string TITLE, a title.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Radius = %f\n', r );
  fprintf ( 1, '  Center = %f  %f\n', center(1:2,1) );

  return
end


function variance = chi_square_noncentral_variance ( a, b )

%*****************************************************************************80
%
%% CHI_SQUARE_NONCENTRAL_VARIANCE returns the variance of the noncentral Chi squared PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, the parameter of the PDF.
%    1 <= A.
%
%    Input, real B, the noncentrality parameter of the PDF.
%    0.0 <= B.
%
%    Output, real VARIANCE, the variance value.
%
  variance = 2.0 * ( a + 2.0 * b );

  return
end


% ANFIS Feed-Forward operation.
function y = anfis_scatter_forward(x,mean,sigma,b,ThetaL4)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%        								 							     				                                       %
%   			                 	NETWORK FUNCTIONALITY SECTION					   %
%      																					                                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[NumInVars NumInTerms] = size(mean);

NumRules = NumInTerms;

% LAYER 1 - INPUT TERM NODES

 In2 = x*ones(1,NumInTerms);
 Out1 = 1./(1 + (abs((In2-mean)./sigma)).^(2*b));

% LAYER 2 - PRODUCT NODES
 Out2 = prod(Out1.',2);
 S_2 = sum(Out2);

 % LAYER 3 - NORMALIZATION NODES
  Out3 = Out2./S_2;

% LAYERSS 4 - 5: CONSEQUENT NODES - SUMMING NODE
 Aux1 = [x; 1]*Out3';

 a = reshape(Aux1,(NumInVars+1)*NumRules,1);  % New Input Training Data shaped as a column vector.

 y = a'*ThetaL4;


% DIAGPROD - Computes the diagonal elements of a matrix product.
%
% D = DIAGPROD(A,B)
%
% Evaluates diag(A*B') efficiently. A and B must have the same size.
%
% D = DIAGPROD(A)
%
% Evaluates diag(A*A') efficiently.

% Last modified 2010-06-24
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function d = diagprod(A,B)

if nargin < 2
  d = dot(A,A,2);
else
  d = dot(A,B,2);
end


function variance = gumbel_variance ( )

%*****************************************************************************80
%
%% GUMBEL_VARIANCE returns the variance of the Gumbel PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = pi * pi / 6.0;

  return
end


function [v,g] = ctransform(x, u, y, p, epsilon)

% ctransform - c-transform and gradient of semi-discrete energy
%
%   [v,g] = ctransform(x,u, y, epsilon);
%
%   u is sampled at points x.
%   v is u^c sampled at points y.
%
%   x must be of size (dim,n)
%   y must be of size (dim,m)
%
% If epsilon==0
%   u^c(y) = min_x  |x-y|^p-u(x)
% else
%   u^c(y) = - epsilon * log( sum_x  exp( (-|x-y|^p + u(x))/epsilon )
%
%   The semi discrete energy between continuous measure a and discrete
%   measure b is
%       E(u) = int u^c(y) da(y) - <u,b>
%   g(:,j) is the gradient of u -> u^c(y_j).
%
%   Copyright (c) 2017 Gabriel Peyre

if nargin<5
    epsilon = 0;
end

n = size(x,2);
m = size(y,2);

c = distmat(x,y).^p; % cost
S = c -  repmat(u(:), [1 m]);
[v,I] = min(S);
if epsilon>0
    % use soft-min, stabilized
    if 1
        S = S - repmat(v, [size(S,1) 1]);
        v = - epsilon * log( sum(  exp( -S/epsilon ) ) ) + v;
    else
        v = - epsilon * log( sum(  exp( -S/epsilon ) ) );
    end
end
v = v(:);

if nargout>1
    % compute also the gradient
    if epsilon==0
        % simply indicator function of the Laguerre cells, indicated by the index I
        g = sparse( I, 1:m, ones(m,1),n, m );
    else
        % smoothed-indicator
        g = exp( -S/epsilon );
        g = g ./ repmat( sum(g), [n 1] );
    end

end

end


function h = p41_h ( n, x )

%*****************************************************************************80
%
%% P41_H evaluates the Hessian for problem 41.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  h = zeros ( n, n );

  h(1,1) = 1200.0 * x(1)^2 - 400.0 * x(2) + 2.0;
  h(1,2) = - 400.0 * x(1);

  h(2,1) = -400.0 * x(1);
  h(2,2) = 220.2;
  h(2,4) = 19.8;

  h(3,3) = -360.0 * x(4) + 1080.0 * x(3)^2 + 2.0;
  h(3,4) = - 360.0 * x(3);

  h(4,2) = 19.8;
  h(4,3) = - 360.0 * x(3);
  h(4,4) = 200.2;

  return
end