function linpack_c_test12 ( )

%*****************************************************************************80
%
%% TEST12 tests CHIFA and CHISL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;
  lda = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST12\n' );
  fprintf ( 1, '  For a single precision complex (C)\n' );
  fprintf ( 1, '  Hermitian matrix (HI):\n' );
  fprintf ( 1, '  CHIFA factors the matrix.\n' );
  fprintf ( 1, '  CHISL solves a linear system.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix order is N = %d\n', n );
%
%  Set the values of the matrix A.
%
  seed = 123456789;

  for i = 1 : n
    [ a(i,i), seed ] = r4_uniform_01 ( seed );
    for j = i+1 : n
      [ a(i,j), seed ] = c4_uniform_01 ( seed );
      a(j,i) = conj ( a(i,j) );
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix A:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  (%8f  %8f)', real ( a(i,j) ), imag ( a(i,j) ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Set the values of the right hand side vector B.
%
  [ x, seed ] = c4vec_uniform_01 ( n, seed );

  b(1:n) = a(1:n,1:n) * transpose ( x(1:n) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The right hand side B:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  (%8f  %8f)\n', real ( b(i) ), imag ( b(i) ) );
  end
%
%  Factor the matrix A.
%
  [ a, ipvt, info ] = chifa ( a, lda, n );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  CHIFA returned an error flag INFO = %d', info );
    return
  end
%
%  Solve the system.
%
  b = chisl ( a, lda, n, ipvt, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Computed                     Exact\n' );
  fprintf ( 1, '  Solution                     Solution\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  (%12f  %12f)  (%12f  %12f)\n', ...
      real ( b(i) ), imag ( b(i) ), real ( x(i) ), imag ( x(i) ) );
  end

  return
end


function triangulation_test32 ( )

%*****************************************************************************80
%
%% TEST32 tests VORONOI_POLYGON_CENTROID.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2006
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;
  neighbor_num = 4;
  node_num = 5;

  center = 5;
  centroid_exact = [ 0.5, 0.5 ];
  neighbor_index = [ 1, 2, 3, 4 ];
  node_xy = [ ...
    0.0, 0.0; ...
    1.0, 0.0; ...
    1.0, 1.0; ...
    0.0, 1.0; ...
    0.5, 0.5 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST32\n' );
  fprintf ( 1, '  VORONOI_POLYGON_CENTROID computes the centroid of\n' );
  fprintf ( 1, '  a finite Voronoi polygon.\n' );

  centroid = voronoi_polygon_centroid ( center, neighbor_num, ...
    neighbor_index, node_num, node_xy );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The computed centroid ( %f  %f )\n', centroid(1:dim_num) );
  fprintf ( 1, '  The correct centroid  ( %f  %f )\n', centroid_exact(1:dim_num) );

  return
end


function [ fvP ] = primalObjD( Xdiag, yvect, Th_vecIdx, lambda1, lambda2, W )
% primal objective (diagonalized)
%  P(W)  sum_i^m ||Xi wi - yi|| + lambda1 ||W||_{1,2} + lambda2/2 ||W||_F^2
fvP = lambda1 * sum(sqrt(sum(W.^2, 2))) + lambda2 /2 * sum(sum(W.^2))...
    + segL2 (Xdiag * W(:) - yvect, Th_vecIdx);

end


%% FUNCTION solve_trace_norm_RMTL
%   trace norm projection
%
%% LICENSE
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Jianhui and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%

function L_hat = solve_trace_norm_RMTL(L, alpha)

[d1 d2] = size(L);

if (d1 > d2)

    [U S V] = svd(L, 0);

    thresholded_value = diag(S) - alpha / 2;

    diag_S = thresholded_value .* ( thresholded_value > 0 );

    L_hat = U * diag(diag_S) * V';

else

    new_L = L';

    [U S V] = svd(new_L, 0);

    thresholded_value = diag(S) - alpha / 2;

    diag_S = thresholded_value .* ( thresholded_value > 0 );

    L_hat = U * diag(diag_S) * V';

    L_hat = L_hat';

end


function l = luma(img)
%
%       l = luma(img)
%
%       This function calculates the luma
%
%
%       input:
%           img: an RGB image
%
%       output:
%           l: luminance as XYZ color
%
%     Copyright (C) 2016  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

col = size(img, 3);

switch col
    case 1
        l = img;

    case 3
        l = 0.299 * img(:,:,1) + 0.587 * img(:,:,2) + 0.114 * img(:,:,3);

    otherwise
        l = mean(img, 3);
        disp('Mean of channels was computed; the input image is not an RGB or luminance image!');
end

end


%This computes the hand-eye calibration using the method described in
%"Hand-Eye Calibration, Horaud, Radu and Dornaika, Fadi"
%
%Input:
%Hmarker2world      a 4x4xNumber_of_Views Matrix of the form
%                   Hmarker2world(:,:,i) = [Ri_3x3 ti_3x1;[ 0 0 0 1]]
%                   with
%                   i = number of the view,
%                   Ri_3x3 the rotation matrix
%                   ti_3x1 the translation vector.
%                   Defining the transformation of the robot hand / marker
%                   to the robot base / external tracking device
%Hgrid2cam          a 4x4xNumber_of_Views Matrix (like above)
%                   Defining the transformation of the grid to the camera
%
%Output:
%Hcam2marker_       The transformation from the camera to the marker /
%                   robot arm
%err                The residuals from the least square processes
%
%Christian Wengert
%Computer Vision Laboratory
%ETH Zurich
%Sternwartstrasse 7
%CH-8092 Zurich
%www.vision.ee.ethz.ch/cwengert
%wengert@vision.ee.ethz.ch
function [Hcam2marker_, err] =inria_calibration(Hmarker2world, Hgrid2cam)

n=size(Hmarker2world,3);
A=[];
B=[];
C=[];

for i=1:n-1
    Dm(:,:,i)=Hgrid2cam(:,:,i+1)*inv(Hgrid2cam(:,:,i));
    Tm(:,:,i)=Dm(1:3,4,i);  % used later to build Matrix B and C

    % equation from the paper:
    % Doff * Dmi->i+1 = inv(Deri+1) * Deri * Doff

    HH(:,:,i)=inv(Hmarker2world(:,:,i+1))*Hmarker2world(:,:,i);   % multiplication HH = Deri+1 * Deri --> quaternion(HH) = z
    z=dcm2q(HH(1:3,1:3,i));
    y=dcm2q(Dm(1:3,1:3,i));  %quaternion of Dm1->i+1

    % quaternion(Doff) * y = z * quaternion(Doff) -> A*quaternion(Doff)=0

    row1=[ z(4)-y(4) -z(3)-y(3)  z(2)+y(2) z(1)-y(1)];
    row2=[ z(3)+y(3)  z(4)-y(4) -z(1)-y(1) z(2)-y(2)];
    row3=[-z(2)-y(2)  z(1)+y(1)  z(4)-y(4) z(3)-y(3)];
    row4=[-z(1)+y(1) -z(2)+y(2) -z(3)+y(3) z(4)-y(4)];

    A=[A; row1; row2; row3; row4];  % build Matrix A

end

[U,S,V]=svd(A);
qoff=V(:,4);
Roff=q2dcm(qoff);
Roff=Roff';                     %  transpone Roff, maybe one of the matrices have wrong direction

% Building B and C matrix
for i=1:n-1
    B=[B; Hmarker2world(1:3,1:3,i+1)-Hmarker2world(1:3,1:3,i)];
    C=[C; Hmarker2world(1:3,4,i)-Hmarker2world(1:3,4,i+1)-Hmarker2world(1:3,1:3,i+1)*Roff*Tm(:,:,i)];
end

Toff=inv(B'*B)*B'*C;

Hcam2marker_=[Roff Toff; 0 0 0 1];

if(nargout==2)
    err = 0;
end


x = uiuc_sample;
x = x(1:256, 1:256);

%% compute roto-translaiton scattering
filt_opt.J = 4;
filt_rot_opt.null = 1;
scat_opt.oversampling = 0;
Wop = wavelet_factory_3d(size(x), filt_opt, filt_rot_opt, scat_opt);

tic;
S_rt = scat(x, Wop);
toc;

%% arrange scattering layer in lexicographic order
var_y{1}.name = 'j';
var_y{1}.index = 1;
var_y{2}.name = 'j';
var_y{2}.index = 2;
var_x{1}.name = 'k2';
var_x{1}.index = 1;
var_x{2}.name = 'theta2';
var_x{2}.index = 1;

big_img = image_scat_layer_order(S_rt{3},var_x, var_y,0);
figure(2);
imagesc(big_img);


function [W,H]=nmfprob(X,K,maxiter,speak)
%
% Probabilistic NFM interpretating X as samples from a multinomial
%
% INPUT:
% X (N,M) : N (dimensionallity) x M (samples) non negative input matrix
% K       : Number of components
% maxiter : Maximum number of iterations to run
% speak   : prints iteration count and changes in connectivity matrix
%           elements unless speak is 0
%
% OUTPUT:
% W       : N x K matrix
% H       : K x M matrix
%
% Lars Kai Hansen, IMM-DTU (c) November 2005
%
print_iter=50;

powers=1.5+(2.5-1.5)*((1:maxiter)-1)/(maxiter-1);
% INITIALIZE
[D,N]=size(X);
X_factor = (sum(sum(X)));
X_org = X;
X=X/X_factor;

W=rand(D,K);
W=W./repmat(sum(W,1),D,1);
H=rand(K,N);
H=H./repmat(sum(H,2),1,N);
P=ones(K,1);
P=P/sum(P);
W1=W;H1=H;

% use W*H to test for convergence
Xr_old = W*H;

for n=1:maxiter,
    %E-step
    Qnorm=(W*diag(P))*H;

    for k=1:K,
        %E-step
        Q=(W(:,k)*H(k,:)*P(k))./(Qnorm+eps);
        XQ=X.*Q;
        %M-step W
        dummy=sum(XQ,2);
        W1(:,k)=dummy/(sum(dummy));
        dummy=sum(XQ,1);
        H1(k,:)=dummy/(sum(dummy));
    end

    W=W1;
    H=H1;

    %%%%%%%%%%%%%%%%%%%%%%%
    % print to screen
    %%%%%%%%%%%%%%%%%%%%%%%
    if (rem(n,print_iter)==0) & speak,
        Xr = W*H;
        diff = sum(sum(abs(Xr_old-Xr)));
        Xr_old = Xr;
        eucl_dist = nmf_euclidean_dist(X_org,W*diag(sqrt(P))*X_factor*diag(sqrt(P))*H);
        errorx = mean(mean(abs(X-W*H)))/mean(mean(X));
        disp(['Iter = ',int2str(n),...
            ', relative error = ',num2str(errorx),...
            ', diff = ', num2str(diff),...
            ', eucl dist ' num2str(eucl_dist)])
        if errorx < 10^(-5), break, end
    end
end,

W=W*diag(sqrt(P))*X_factor;
H=diag(sqrt(P))*H;


function [graphW, NNIndex] = gacBuildDigraph_c(distance_matrix, K, a)
%% Build directed graph
% Input:
%   - distance_matrix: pairwise distances, d_{i -> j}
%   - K: the number of nearest neighbors for KNN graph
%   - a: for covariance estimation
%       sigma^2 = (\sum_{i=1}^n \sum_{j \in N_i^3} d_{ij}^2) * a
% Output:
%   - graphW: asymmetric weighted adjacency matrix,
%               w_{ij} = exp(- d_{ij}^2 / sig2), if j \in N_i^K
%	- NNIndex: (2K) nearest neighbors, N x (2K+1) matrix
% by Wei Zhang (wzhang009 at gmail.com), June, 8, 2011
%
% Please cite the following paper, if you find the code is helpful
%
% W. Zhang, X. Wang, D. Zhao, and X. Tang.
% Graph Degree Linkage: Agglomerative Clustering on a Directed Graph.
% in Proceedings of European Conference on Computer Vision (ECCV), 2012.

%% NN indices
N = size(distance_matrix,1);
% find 2*K NNs in the sense of given distances
[sortedDist,NNIndex] = gacMink(distance_matrix,max(K+1,4),2);

%% estimate derivation
sig2 = mean(mean(sortedDist(:,2:max(K+1,4)))) * a;
%%%%%%%%%
tmpNNDist = min(sortedDist(:,2:end),[],2);
while any(exp(- tmpNNDist / sig2) < 1e-5) % check sig2 and magnify it if it is too small
    sig2 = 2*sig2;
end
%%%%%%%%%
disp(['  sigma = ' num2str(sqrt(sig2))]);

%% build graph
ND = sortedDist(:, 2:K+1);
NI = NNIndex(:, 2:K+1);
XI = repmat([1:N]', 1, K);
graphW = full(sparse(XI(:),NI(:),exp(-ND(:)*(1/sig2)), N, N));
graphW(1:N+1:end) = 1;

end


%%********************************************************************
%% igmres: compute ideal GMRES polynomial of deg m
%%         of a matrix B.
%%
%%    min || p(B) ||_2
%%    where p is a polynomial of degree <= m and p(0) = 1.
%%
%%    B = a general square matrix.
%%-----------------------------------------------------------
%%
%% [blk,Avec,C,b,X0,y0,Z0,objval,p] = igmres(B,m,feas,solve);
%%
%% B = a square matrix.
%% m = degree of polynomial.
%% feas  = 1 if want feasible starting point
%%       = 0 if otherwise.
%% solve = 0 just to initialize
%%       = 1 if want to solve the problem.
%%
%% p = ideal GMRES polynomial in Matlab format.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

 function [blk,Avec,C,b,X0,y0,Z0,objval,p] = igmres(B,m,feas,solve)

   if (nargin <= 3); solve = 0; end;
   if (nargin <= 2); feas  = 0; end;

   N = length(B);
   if (m >= N); error('degree >= size of B'); end;

   A = cell(1,m+1);
   [V,H,R] = orthbasis(B,m-1);
   A{1} = eye(N);
   for k = [1:m];  A{k+1} = B*V{k};  end;
   [blk,Avec,C,b,X0,y0,Z0,objval,xx] = norm_min(A,feas,solve);
   if (solve)
      y = xx;
      x1 = R(1:m,1:m)*y(1:m);
      p = [x1(m:-1:1); 1];
   else
      objval = []; p = [];
   end
%%********************************************************************


function out = ALM_SADAL_smoothed(D, opts)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test ALM in the paper ''Fast Alternating Linearization Methods for
%       Minimizing the Sum of Two Convex Functions'', Donald Goldfarb,
%       Shiqian Ma and Katya Scheinberg, Tech. Report, Columbia University,
%       2009 - 2010.
%
% Author: Shiqian Ma
% Date  : Apr. 20, 2010
% IEOR, Columbia University, Copyright (2010)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[m,n] = size(D);
mu = opts.mu; sigma = opts.sigma; rho = opts.rho;
Y = zeros(m,n); gradgY = zeros(m,n);
X = D - Y; Dnorm = norm(D,'fro');
Lambda = zeros(m,n); sv = opts.sv;

for itr = 1: opts.maxitr
    if choosvd(n, sv) == 1
        [U, gamma, V] = lansvd(mu*Lambda-Y+D,sv,'L');
    else
        [U, gamma, V] = svd(mu*Lambda-Y+D, 'econ');
    end
    gamma = diag(gamma);
    gamma_new = gamma-mu*gamma./max(gamma,mu+sigma);
    svp = length(find(gamma > mu));
    if svp < sv
        sv = min(svp + 1, n);
    else
        sv = min(svp + round(0.05*n), n);
    end

    Xp = X;
    X = U*diag(gamma_new)*V';
    Lambda = Lambda - (X+Y-D)/mu;
    muY = mu;
    B = Lambda - (X-D)/muY;
    Yp = Y;

    Y = muY*B - muY*min(rho, max(-rho,muY*B/(sigma+muY)));
    Lambda = Lambda - (X+Y-D)/muY;

    StopCrit = norm(D-X-Y,'fro')/Dnorm;
    relX = norm(X-opts.Xs,'fro')/norm(opts.Xs,'fro');
    relY = norm(Y-opts.Ys,'fro')/norm(opts.Ys,'fro');
%     fprintf('iter: %d, mu: %3.2e, rank(X):%d, relX: %3.2e, relY: %3.2e,crit:%3.2e\n', ...
%         itr, mu, length(find(gamma_new>1e-3)), relX, relY, norm(D-X-Y,'fro')/norm(D,'fro'));
    %fprintf('iter: %d, mu: %3.2e, crit:%3.2e\n', itr, mu, norm(D-X-Y,'fro')/norm(D,'fro'));

    mu = max(opts.muf, mu*opts.eta_mu);
    sigma = max(opts.sigmaf, sigma*opts.eta_sigma);

    if StopCrit < opts.epsilon
        out.X = X; out.Y = Y; out.iter = itr; out.relX = relX; out.relY = relY; out.StopCrit = StopCrit;
        return;
    end
end

out.X = X;
out.Y = Y;
out.iter = itr;
out.relX = relX;
out.relY = relY;
out.StopCrit = StopCrit;


function img = discreteCylinder(varargin)
%DISCRETECYLINDER Discretize a 3D cylinder
%
%   IMG = discreteCylinder(LX, LY, LZ, P1, P2, RADIUS)
%   LX, LY and LZ are row vectors specifying position of vertex centers
%   along each coordinate.
%   P1 is the starting point of the cylinder, given as a 1-by-3 row vector
%   P2 is the ending point of the cylinder, given as a 1-by-3 row vector
%   RADIUS is the cylinder radius.
%
%   IMG = discreteCylinder(LX, LY, LZ, CYLINDER)
%   send parameters in a row vector, where CYLINDER is a 1-by-7 row vector
%   containing coordinates of start point, of end point, and radius:
%   CYLINDER = [X1 Y1 Z1 X2 Y2 Z2 R];
%
%   Example
%     % Compute the union of three mutually orthogonal cylinders
%     p0 = [30 30 30];
%     p1 = [90 30 30];
%     p2 = [30 90 30];
%     p3 = [30 30 90];
%     cyl1 = discreteCylinder(1:100, 1:100, 1:100, [p0 p1 25]);
%     cyl2 = discreteCylinder(1:100, 1:100, 1:100, [p0 p2 25]);
%     cyl3 = discreteCylinder(1:100, 1:100, 1:100, [p0 p3 25]);
%     cylUnion = cyl1 | cyl2 | cyl3;
%     [f v] = isosurface(cylUnion, .5);
%     figure;
%     drawMesh(v, f, 'linestyle', 'none', 'facecolor', 'r');
%     l = light; view([120 20]);
%
%   See also
%   imShapes, discreteBall, discreteCapsule3d
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2010-10-21
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY
%   2010-10-21 create from discreteCube

%% Process input arguments

% compute coordinate of image voxels
[lx, ly, lz, varargin] = parseGridArgs3d(varargin{:});
[x, y, z] = meshgrid(lx, ly, lz);

% process input parameters
if length(varargin) == 1
    % input is a 1-by-7 row vector
    var = varargin{1};
    if length(var) ~= 7
        error('Should specify a row vector with 7 inputs');
    end

    % extract first and last point coordinates
    p1 = var(1:3);
    p2 = var(4:6);
    radius = var(7);

elseif length(varargin) == 3
    % inputs are P1, P2 and R
    p1 = varargin{1};
    p2 = varargin{2};
    radius = varargin{3};

else
    error('wrong number of arguments: should be 1 or 3');
end

%% Transform voxel coordinates

% compute cylinder direction angle
dirVect = p2-p1;

[theta, phi, height] = cart2sph2(dirVect);

% compute coordinate of image voxels in cylinder reference system
% (cylinder pointing upwards)
trans = composeTransforms3d(...
    createTranslation3d(-p1),...
    createRotationOz(-phi),...
    createRotationOy(-theta),...
    createScaling3d(1./[radius radius height]));
[x, y, z] = transformPoint3d(x, y, z, trans);

%% Compute final image

% create image: simple threshold over 3 dimensions, and test z axis
img = ((x.*x + y.*y) < 1) & (z>0) & (z<1);


function [nyquist_number,Bezugsfrequenz,Signal_amp] = Fast_Harmonics(x,fs,duration)
xx = 0;
if xx == 1
    % Sampling
    fs = 1000;     % Sampling rate [Hz]
    Ts = 1/fs;     % Sampling period [s]
    fNy = fs / 2;  % Nyquist frequency [Hz]
    duration = 10; % Duration [s]
    t = 0 : Ts : duration-Ts; % Time vector
    noSamples = length(t);    % Number of samples
    % Original signal
    x = 220.*sin(2 .* pi .* 50 .* t);
    % Harmonics
    x1 = 100.*sin(2 .* pi .* 100 .* t);
    x2 = 100.*sin(2 .* pi .* 200 .* t);
    x3 = 100.*sin(2 .* pi .* 300 .* t);
    % Contaminated signal
    xn = x + x1 + x2 + x3;
    % Frequency analysis
    f = 0 : fs/noSamples : fs - fs/noSamples; % Frequency vector
    % FFT
    x_fft = abs(fft(x));
    xn_fft = abs(fft(xn));
    % Plot
    figure(1);
    subplot(2,2,1);
    plot(t, x);
    subplot(2,2,2);
    plot(t, xn);
    subplot(2,2,3);
    plot(f,x_fft);
    xlim([0 fNy]);
    subplot(2,2,4);
    plot(f,xn_fft);
    xlim([0 fNy]);
else
%     x is the sampled data
%     m = length(x) is the window length (number of samples)
%     fs is the samples/unit time
%     dt = 1/fs is Time increment per sample
%     t = (0:m-1)/fs Time range for data
%     y = fft(x,n) is the Discrete Fourier Transform (DFT)
%     abs(y) is the Amplitude of the DFT
%     (abs(y).^2)/n is power of the DFT
%     fs/n Frequency increment
%     f = (0:n-1)*(fs/n) is frequency range
%     fs/2 is Nyquist frequency
%     n = pow2(nextpowe2(m)) is the transfrom length
    Ts = 1/fs;     % Sampling period [s]
    fNy = fs / 2;  % Nyquist frequency [Hz]
    t = 0 : Ts : duration-Ts; % Time vector
    noSamples = length(t);    % Number of samples
    % Original signal
    % Frequency analysis
    f = 0 : fs/noSamples : fs - fs/noSamples; % Frequency vector
    % FFT
    x_fft = abs(fft(x));
    xx = x';
    X = fft(xx(2:end));
    if rem(length(X),2)==0
        nyquist_number = length(X)/2;
    else
        nyquist_number = (length(X)+1)/2;
    end
    Bezugsfrequenz = 1/(t(end)-t(1));
    Signal_amp        = zeros(1,nyquist_number+1);
    Signal_phi        = zeros(1,nyquist_number+1);
    Signal_amp(1)     = X(1)/length(X);									                % Gleichanteil des Signals [unit]
    Signal_amp(2:end-1) = 2*abs(X(2:nyquist_number))/length(X);
    Signal_amp(end) = abs(X(nyquist_number+1))/length(X);
    %Signal_amp(2:end) = [2*abs(X(2:nyquist_number)) abs(X(nyquist_number+1))]/length(X);   % Amplituden der Harmonischen des Signals [unit]
    Signal_phi(1)     = 0;
    Signal_phi(2:end) = angle(X(2:nyquist_number+1));		                                % Phasenwinkel der Harmonischen des Signals [rad]

    %xn_fft = abs(fft(xn));
    % Plot
end


function H=calcSpherConvHessian(zSpher,systemType,useHalfRange,lTx,lRx,M)
%%CALCSPHERCONVHESSIAN Calculate the Hessian (a matrix of second partial
%            derivatives) for a monostatic or bistatic range and a
%            spherical direction measurement in 3D, ignoring atmospheric
%            effects, with respect to Cartesian position. This type of
%            Jacobian is useful when performing tracking using Cartesian-
%            converted measurements where the clutter density is specified
%            in the measurement coordinate system, not the converted
%            measurement coordinate system.
%
%INPUTS: zSpher A 3X1 point in range and azimuth and angle in the format
%           [range;azimuth;angle], where the angles are given in radians.
% systemType An optional parameter specifying the axes from which the
%           angles are measured in radians. Possible values are
%           0 (The default if omitted) Azimuth is measured
%             counterclockwise from the x-axis in the x-y plane. Elevation
%             is measured up from the x-y plane (towards the z-axis). This
%             is consistent with common spherical coordinate systems for
%             specifying longitude (azimuth) and geocentric latitude
%             (elevation).
%           1 Azimuth is measured counterclockwise from the z-axis in the
%             z-x plane. Elevation is measured up from the z-x plane
%             (towards the y-axis). This is consistent with some spherical
%             coordinate systems that use the z axis as the boresight
%             direction of the radar.
%           2 This is the same as 0 except instead of being given
%             elevation, one desires the angle away from the z-axis, which
%             is (pi/2-elevation).
%           3 This is the same as 0 except azimuth is measured clockwise
%             from the y-axis in the x-y plane instead of counterclockwise
%             from the x-axis. This coordinate system often arises when
%             given "bearings" in a local East-North-Up coordinate system,
%             where the bearing directions are measured East of North.
% useHalfRange An optional boolean value specifying whether the bistatic
%           (round-trip) range value has been divided by two. This normally
%           comes up when operating in monostatic mode (the most common
%           type of spherical coordinate system), so that the range
%           reported is a one-way range (or just half a bistatic range).
%           The default if this parameter is not provided is false if lTx
%           is provided and true if it is are omitted (monostatic). If no
%           range values are provided, an empty matrix can be passed.
%       lTx The 3X1 [x;y;z] location vector of the transmitter in global
%           Cartesian coordinates. If this parameter is omitted or an
%           empty matrix is passed, then the transmitter is assumed to be
%           at the origin.
%       lRx The 3X1 [x;y;z] location vector of the receiver in Cartesian
%           coordinates. If this parameter is omitted or an empty matrix
%           is passed, then the receiver is assumed to be at the origin.
%         M A 3X3 rotation matrix to go from the alignment of the global
%           coordinate system to that at the receiver. The z-axis of the
%           local coordinate system of the receiver is the pointing
%           direction of the receiver. If omitted or an empty matrix is
%           passed, then it is assumed that the local coordinate system is
%           aligned with the global and M=eye(3) --the identity matrix is
%           used.
%
%OUTPUTS: H The 3X3X3 Hessian matrix, where H(:,:,1) is the Hessian for the
%           range component, H(:,:,2) is the Hessian for the azimuth
%           component, and H(:,:,3) is the Hessian for the elevation
%           component. The ordering of the derivatives in each matrix is:
%                  [d^2/(dxdx), d^2/(dxdy), d^2/(dxdz);
%                   d^2/(dydx), d^2/(dydy), d^2/(dydz);
%                   d^2/(dzdx), d^2/(dzdy), d^2/(dzdz)];
%          note that each matrix is symmetric (i.e.
%                   d^2/(dydx)=d^2/(dxdy) ).
%
%This function just calls spher2Cart and then calcSpherHessian.
%
%June 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<6||isempty(M))
	M=eye(3,3);
end

if(nargin<5)
   lRx=[];
end

if(nargin<4)
    lTx=[];
end

if(isempty(lTx)&&(nargin<3||isempty(useHalfRange)))
    useHalfRange=true;
elseif(~isempty(lTx)&&(nargin<3||isempty(useHalfRange)))
    useHalfRange=false;
end

if(nargin<2||isempty(systemType))
    systemType=0;
end

x=spher2Cart(zSpher,systemType,useHalfRange,lTx,lRx,M);
H=calcSpherHessian(x,systemType,useHalfRange,lTx,lRx,M);

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 phi = Segment2D_public( wb, wr, lambda, phi0, Nit )
% wb>0 edge detector
% wr region term
% lambda weight of region term over edge term
% phi0 initialization
% Nit number of iterations

verb = 1; fig_it = figure;
[Nx,Ny] = size(wb);
N = Nx*Ny;

% Parameters of augemented Lagrangian and minimizers
r1 = 1;
r2 = 1;
r3 = 0.1;
r4 = 0.1;
Njvarphi = 3; % increase it to get a better approximation to the minimizar with respect to varphi
Njphi = 2; % increase it to get better approximation to | grad phi | = 1
epsilon = 1; % controls smoothness of approximation to Heaviside and Dirac

% usufelu for FFT-based minimization
[Y,X] = meshgrid(0:Ny-1,0:Nx-1);
auxFFT = cos(2*pi/Nx*X)+cos(2*pi/Ny*Y)-2;

% initialize Lagrange multipliers and splitting variables
l1 = zeros(Nx,Ny);
l2x = zeros(Nx,Ny);
l2y = zeros(Nx,Ny);
l3 = zeros(Nx,Ny);
l4x = zeros(Nx,Ny);
l4y = zeros(Nx,Ny);
phi = phi0;
varphi = phi0;
u = H(phi0,epsilon);
dx = Fx(u);
dy = Fy(u);
px = Fx(phi);
py = Fy(phi);

wr = -lambda* wr;
% Start iterations: minimize w.r.t. each variable and update lagrange multipliers
for i=1:Nit

    % u^n+1 solved in Fourier domain
    b = r1*(H(varphi,epsilon)- l1/r1) - wr -r2* ( Bx(dx+l2x/r2)+By(dy+l2y/r2) );
    u = real(ifft2( fft2( b )./( -2*r2*auxFFT + r1 )));

    % d^n+1 solved by shrinkage
    Fxu = Fx(u);
    Fyu = Fy(u);
    [dx,dy] = shrink(Fxu - l2x/r2,Fyu - l2y/r2,wb/r2);

    % varphi^n+1 approx initialization and Newton method
    v = u + l1/r1;
    z = phi- l3/r3;
    E_0 = 0.5*r3*z.^2 + 0.5*r1*( 0.5 - v ).^2;
    E_z = 0.5*r1*( H(z,epsilon) - v ).^2;
        % initialize with solution to ideal Heaviside
    cond_m2 = ( z>=0 ).*( v>=0.5 ) + ( z<0 ).*( v<0.5 );
    varphi = z.*cond_m2 + (1-cond_m2).*( E_z < E_0 ).*z;
        % newton method to find zeros of derivative
    for j=1:Njvarphi
         df = r3*( varphi - z) + r1*( H(varphi,epsilon) - v ).*D(varphi,epsilon);
         hf = r3 + r1*D(varphi,epsilon).*D(varphi,epsilon) + r1*(H(varphi,epsilon) - v ).*dD(varphi,epsilon);
         varphi = varphi - df./( hf + (abs(hf)<1e-3) );
    end

    % lambda^n+1 update lagrangians
    l1 = l1 + r1* (u-H(varphi,epsilon));
    l2x = l2x + r2* (dx-Fxu);
    l2y = l2y + r2* (dy-Fyu);
    l3 = l3 + r3* (varphi- phi);

    %%%%%%%%%%%%%%%%%%%%%%%%%
    for jphi=1:Njphi

        % p^n+1 solved projecting in sphere
        ex = Fx(phi)-l4x/r4;
        ey = Fy(phi)-l4y/r4;
        norm_e = sqrt( ex.^2 + ey.^2 );
        px = ex./(norm_e + (norm_e==0));
        py = ey./(norm_e + (norm_e==0));

        % phi^n+1 solved in Fourier domain
        b = r3* (varphi+l3/r3) -r4.* ( Bx(px+l4x/r4)+By(py+l4y/r4) );
        phi = real(ifft2( fft2( b )./( -2*r4.*auxFFT + r3 )));

        % lambda^n+1 update lagrangians
        l4x = l4x + r4.*(px-Fx(phi));
        l4y = l4y + r4.*(py-Fy(phi));
    end

    %plotting
    if ((rem(i,5)==0)&&(verb==1))
        pause(0.01);
        figure(fig_it)
        clf; imagesc(phi); colormap(gray); colorbar;
        title([num2str(i), ' iterations']); hold on; contour(phi,[0 0],'m');
        for ct=-2:2; contour(phi,[ct ct],'k'); end; contour(phi,[0 0],'m');
    end
end

return;

% shrinkage function
function [xs,ys] = shrink(x,y,lambda)

s = sqrt(x.*conj(x)+y.*conj(y));
ss = s-lambda;
ss = ss.*(ss>0);

s = s+(s<lambda);
ss = ss./s;

xs = ss.*x;
ys = ss.*y;

return;

% smooth approximation of Heaviside function
function v = H(phi,epsilon)
v = 0.5*( 1 + 2/pi*atan(phi/epsilon) );
return;

% smooth approximation of Dirac distribution
function v = D(phi,epsilon)
v = 1/pi* epsilon./ (epsilon^2 + phi.^2);
return;

% smooth approximation of derivative of the Dirac distribution
function v = dD(phi,epsilon)
v = -2/pi* epsilon*phi./ (epsilon^2 + phi.^2).^2;

% Forward derivative operator on x with boundary condition u(:,1,:)=u(:,Nx,:)
function Fxu = Fx(u)
[Ny,Nx] = size(u);
Fxu = circshift(u,[0 -1])-u;
Fxu(:,Nx) = zeros(Ny,1);

% Forward derivative operator on y with boundary condition u(1,:,:)=u(Ny,:,:)
function Fyu = Fy(u)
[Ny,Nx] = size(u);
Fyu = circshift(u,[-1 0])-u;
Fyu(Ny,:) = zeros(1,Nx);

% Backward derivative operator on x with boundary condition Bxu(:,1)=u(:,1)
function Bxu = Bx(u)
[Ny,Nx] = size(u);
Bxu = u - circshift(u,[0 1]);
Bxu(:,1) = u(:,1);
Bxu(:,Nx) = -u(:,Nx-1);

% Backward derivative operator on x with boundary condition Bxu(1,:)=u(1,:)
function Byu = By(u)
[Ny,Nx] = size(u);
Byu = u - circshift(u,[1 0]);
Byu(1,:) = u(1,:);
Byu(Ny,:) = -u(Ny-1,:);


% This function is borrowed from rastamat

function d = deltas(x, w)
% D = deltas(X,W)  Calculate the deltas (derivatives) of a sequence
%    Use a W-point window (W odd, default 9) to calculate deltas using a
%    simple linear slope.  This mirrors the delta calculation performed
%    in feacalc etc.  Each row of X is filtered separately.
% 2003-06-30 dpwe@ee.columbia.edu

if nargin < 2
  w = 9;
end

[nr,nc] = size(x);

if nc == 0
  % empty vector passed in; return empty vector
  d = x;

else
  % actually calculate deltas

  % Define window shape
  hlen = floor(w/2);
  w = 2*hlen + 1;
  win = hlen:-1:-hlen;

  % pad data by repeating first and last columns
  xx = [repmat(x(:,1),1,hlen),x,repmat(x(:,end),1,hlen)];

  % Apply the delta filter
  d = filter(win, 1, xx, [], 2);  % filter along dim 2 (rows)

  % Trim edges
  d = d(:,2*hlen + [1:nc]);

end


function point_total_num = sparse_grid_mixed_size_total ( dim_num, level_max, rule )

%*****************************************************************************80
%
%% SPARSE_GRID_MIXED_SIZE sizes a sparse grid, counting duplicate points.
%
%  Discussion:
%
%    The sparse grid is the logical sum of product grids with total LEVEL
%    between LEVEL_MIN and LEVEL_MAX.
%
%    In some cases, the same point may occur in different product grids
%    used to form the sparse grid.
%
%    This routine counts the total number of points used to construct the sparse
%    grid; if the same point occurs several times, each occurrence is added
%    to the sum.
%
%    This computation is useful in order to be able to allocate enough
%    space for the full set of points, before they are compressed by removing
%    duplicates.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Input, integer RULE(DIM_NUM), the rule in each dimension.
%     1, "CC",  Clenshaw Curtis, Closed Fully Nested rule.
%     2, "F2",  Fejer Type 2, Open Fully Nested rule.
%     3, "GP",  Gauss Patterson, Open Fully Nested rule.
%     4, "GL",  Gauss Legendre, Open Weakly Nested rule.
%     5, "GH",  Gauss Hermite, Open Weakly Nested rule.
%     6, "GGH", Generalized Gauss Hermite, Open Weakly Nested rule.
%     7, "LG",  Gauss Laguerre, Open Non Nested rule.
%     8, "GLG", Generalized Gauss Laguerre, Open Non Nested rule.
%     9, "GJ",  Gauss Jacobi, Open Non Nested rule.
%    10, "GW",  Golub Welsch, (presumed) Open Non Nested rule.
%    11, "CC_SE", Clenshaw Curtis Slow Exponential, Closed Fully Nested rule.
%    12, "F2_SE", Fejer Type 2 Slow Exponential, Closed Fully Nested rule.
%    13, "GP_SE", Gauss Patterson Slow Exponential, Closed Fully Nested rule.
%    14, "CC_ME", Clenshaw Curtis Moderate Exponential, Closed Fully Nested rule.
%    15, "F2_ME", Fejer Type 2 Moderate Exponential, Closed Fully Nested rule.
%    16, "GP_ME", Gauss Patterson Moderate Exponential, Closed Fully Nested rule.
%    17, "CCN", Clenshaw Curtis Nested, Linear, Closed Fully Nested rule.
%
%    Output, integer POINT_TOTAL_NUM, the total number of points in the grid.
%

%
%  Special case.
%
  if ( level_max == 0 )
    point_total_num = 1;
    return
  end

  point_total_num = 0;
%
%  The outer loop generates values of LEVEL.
%
  level_min = max ( 0, level_max + 1 - dim_num );

  for level = level_min : level_max
%
%  The middle loop generates a GRID,
%  based on the next partition that adds up to LEVEL.
%
    level_1d = [];
    more_grids = 0;
    h = 0;
    t = 0;

    while ( 1 )

      [ level_1d, more_grids, h, t ] = comp_next ( level, dim_num, level_1d, ...
        more_grids, h, t );

      order_1d = level_to_order_default ( dim_num, level_1d, rule );

      point_total_num = point_total_num + prod ( order_1d(1:dim_num) );

      if ( ~more_grids )
        break
      end

    end

  end

  return
end


%% Random Search Algorithm (Pure Random Search Algorithm)

% This code finds the minimum of f(x) = x(1)^2 + x(2)^2
% in which -5 < x(i) < 5
% This function is a convex and the minimum is at (0,0)
% The RSA is the simplest algorithm to solve optimization problem
% it is not efficient and it sometimes cannot solve the problem

clc
close all
clear all
dim=2;
popsize=100;
ftarget=0.01;
numIter=100;
ObjFun=@(x) sum(x.^2);
for i=1:numIter
    candidate=10*rand(dim,popsize)-5;
    best=min(feval(ObjFun,candidate));
    if best <= ftarget
        break;
    end
end
disp(best);


function [GHz] = kHz2GHz(kHz)
% Convert frequency from kilohertz to gigahertz.
% Chad A. Greene 2012
GHz = kHz*1e-6;


function ring_render(ring_param,x_list,y_list)

%Extract ring parameters:
%------------------------
Ri=ring_param{2}; %inner radius
Ro=ring_param{3}; %outer modulation size
H=ring_param{4}; %height
Rfreq=ring_param{1}; %modulation frequency
face_color=ring_param{5}/255; %color

delta_theta=5; %angle spacing (smaller values will increase render time)
theta=0:delta_theta:360;

%Complete the list of cross-section nodes:
%-----------------------------------------
x=x_list;
y=y_list;
y_new=[x(1:length(x)-1) x(1)];
z_new=[y(1:length(y)-1) y(1)];
x_new=zeros(size(y));
x=x_new;
y=y_new+Ri;
z=z_new;
clear x_new y_new z_new

%Render ring:
%------------
for nt=1:length(theta)-1 %loop through each face angle

    th1=theta(nt);
    th2=theta(nt+1);

    for np=1:length(x)-1 %generate the edges of the face

        xtemp=x(np:np+1);
        ytemp=y(np:np+1);
        ztemp=z(np:np+1);

        %apply modulation to outer cross-section coordinates:
        r_temp1=(ytemp>Ri)*Ro*((sind(th1*Rfreq)+1)/2);
        r_temp2=(ytemp>Ri)*Ro*((sind(th2*Rfreq)+1)/2);

        %apply rotation transformation to cross-section coordinate:
        xtemp_rot1=xtemp*cosd(th1)+(ytemp+r_temp1)*sind(th1);
        xtemp_rot2=xtemp*cosd(th2)+(ytemp+r_temp2)*sind(th2);
        ytemp_rot1=xtemp*-sind(th1)+(ytemp+r_temp1)*cosd(th1);
        ytemp_rot2=xtemp*-sind(th2)+(ytemp+r_temp2)*cosd(th2);

        %edge lists:
        xtemp_face=[xtemp_rot1(1) xtemp_rot2(1) xtemp_rot2(2) xtemp_rot1(2) xtemp_rot1(1)];
        ytemp_face=[ytemp_rot1(1) ytemp_rot2(1) ytemp_rot2(2) ytemp_rot1(2) ytemp_rot1(1)];
        ztemp_face=[ztemp(1) ztemp(1) ztemp(2) ztemp(2) ztemp(1)];

        %render face:
        hf=fill3(xtemp_face,ytemp_face,ztemp_face,face_color);
        set(hf,'EdgeAlpha',0,'FaceLighting','phong','BackFaceLighting','lit','SpecularStrength',1,'FaceAlpha',.5)
        material shiny
        hold on
    end
end
axis equal
axis([-1 1 -1 1 -1 1]*(Ri+H))
axis off
set(gcf,'Color','k')
light('Position',[1 1 1]*25,'Style','local')
light('Position',[-1 1 -1]*25,'Style','local')
light('Position',[1 -1 -1]*25,'Style','local')
light('Position',[-1 -1 1]*25,'Style','local')


function [KFSmatrices] = makeMatricesSLMfullSV(dataValues,p)
%% Purpose
%  This function calculates the required matrices for the Kalman Filter for the SLM full SV model.
% The model is Y

%% Initialize
Ppsi=dataValues.Ppsi;
[ns,n]=size(Ppsi);

%% Z: coefficients of measurment equation
Ztemp=zeros(ns+n,n*(p+1));
Ztemp(ns+1:end,1:2*n)=[eye(n) eye(n)];
Ztemp(1:ns,1:n)= Ppsi;
KFSmatrices.Zmatrix=Ztemp;

%% R: for variance covariance in transition equation
KFSmatrices.R=[eye(2*n);zeros(n*(p-1),2*n)];

%% L: for variance covariance in measurement equation
KFSmatrices.L=[eye(ns);zeros(n,ns)];

%% Tmatrix: coefficients in transition equation
TmatrixTemp=zeros(n*(p+1),n*(p+1));
TmatrixTemp(1:n,1:n) = eye(n);
%TmatrixTemp(n+1:2*n,n+1:end) = Bhat';
TmatrixTemp(2*n+1:end,n+1:end-n) = eye(n*(p-1));

KFSmatrices.Tmatrix=TmatrixTemp;

end


function xyz = cube2ball(XYZ)
%
% transforms cubochoric coordinates of quaternions into homochoric ones
% maps from cube (edge pi^(2/3) onto ball (radius (3*pi/4)^(1/3))
%
% Input
%  XYZ - cubochoric coordinates (X,Y,Z) of N points of the cube
%
% Output
%  xyz - homochoric coordinates (x,y,z) of N points of the ball

% the actual mapping is only defined on one pyramid Pz (z>=abs(x),z>=abs(y))
% map other points by:
%  1. transform coordinates, so that we get a point of Pz
%  2. map the point
%  3. apply the inverse transformation

% define permutaions (and inverse ones) for each region (pyramid)
p = regionId(XYZ);
permRegion  = [2 3 1;3 2 1;1 3 2;3 1 2;1 2 3;2 1 3];
ipermRegion = [3 1 2;3 2 1;1 3 2;2 3 1;1 2 3;2 1 3];

%  permXYZ contains for each point its permutation
% ipermXYZ contains for each point its inverse permutation

permXYZ  =  permRegion(p,:);
ipermXYZ = ipermRegion(p,:);

% apply the permutation on each grid point
XYZ = XYZ(sub2ind(size(XYZ), (1:size(XYZ,1)).' * [1 1 1] ,permXYZ));

% map each point
cosyx = sqrt(2) * cos(pi/12 * XYZ(:,2)./XYZ(:,1));
sinyx = sqrt(2) * sin(pi/12 * XYZ(:,2)./XYZ(:,1));

A = 2 * cosyx - 3;
B = 2 - cosyx;
C = (6/pi)^(1/3);
D = sqrt(A .* (XYZ(:,1).^2 - XYZ(:,3).^2) + XYZ(:,3).^2);
E = C * XYZ(:,1)./abs(XYZ(:,3)) .* D ./ B;

xyz = XYZ;
xyz(:,1) = E .* (cosyx - 1);
xyz(:,2) = E .* sinyx;
xyz(:,3) = C * (XYZ(:,3) + XYZ(:,1).^2 ./ XYZ(:,3) .* A ./ B);

% overwrite points with division by zero
O = (XYZ(:,1)==0);
xyz(O,:) = (6/pi)^(1/3) * XYZ(O,:);

xyz = xyz(sub2ind(size(xyz), (1:size(xyz,1)).' * [1 1 1] ,ipermXYZ));

end


function featAugm = get_augm_spatial_features_diff_neighbour_locref(feat)

% relative coord of 2 detections
delta = feat(:, 1:2);
a = compute_angle(delta(:,1), delta(:,2));

delta_forward = feat(:, 5:6);
a_forward = compute_angle(delta_forward(:,1), delta_forward(:,2));

delta1 = delta - delta_forward;
dist1 = sqrt(delta1(:,1).^2 + delta1(:,2).^2);
a1 = wrap_angle(a - a_forward);
abs_a1 = abs(a1);
%abs_a1(:) = 0;

% now the same for backward
delta = feat(:, 3:4);
a = compute_angle(-delta(:,1), -delta(:,2));

delta_backward = feat(:, 7:8);
a_backward = compute_angle(delta_backward(:,1), delta_backward(:,2));

delta2 = delta - delta_backward;
dist2 = sqrt(delta2(:,1).^2 + delta2(:,2).^2);
a2 = wrap_angle(a - a_backward);
abs_a2 = abs(a2);

%featAugm = cat(2, dist, exp(-dist), dx, dy, abs(dx), abs(dy), dxdy, dx.^2, dy.^2, abs_a, a.^2, exp(-abs_a));

featAugm = cat(2, dist1, abs_a1, dist2, abs_a2);
featAugm = cat(2, featAugm, exp(-featAugm));

end

function angle = compute_angle(deltaX, deltaY)
angle = atan2(deltaY,deltaX);
angle = wrapMinusPiPifast(angle);
assert(all(angle <= pi) && all(angle >= -pi));
end

function a = wrap_angle(a)
larger = a > pi;
smaller = a < -pi;
a(larger)  = a(larger) - 2*pi;
a(smaller) = a(smaller)+ 2*pi;
end


function hydro = radiationIRF(hydro,tEnd,nDt,nDw,wMin,wMax)
% Calculates the normalized radiation impulse response function. This is
% equivalent to the radiation IRF in the theory section normalized by
% :math:`\rho`:
%
%     :math:`\overline{K}_{r,i,j}(t) = {\frac{2}{\pi}}\intop_0^{\infty}{\frac{B_{i,j}(\omega)}{\rho}}\cos({\omega}t)d\omega`
%
% Default parameters can be used by inputting [].
% See ``WEC-Sim\examples\BEMIO`` for examples of usage.
%
% Parameters
% ----------
%     hydro : struct
%         Structure of hydro data
%
%     tEnd : float
%         Calculation range for the IRF, where the IRF is calculated from t
%         = 0 to tEnd, and the default is 100 s
%
%     nDt : float
%         Number of time steps in the IRF, the default is 1001
%
%     nDw : float
%         Number of frequency steps used in the IRF calculation
%         (hydrodynamic coefficients are interpolated to correspond), the
%         default is 1001
%
%     wMin : float
%         Minimum frequency to use in the IRF calculation, the default is
%         the minimum frequency from the BEM data
%
%     wMax : float
%         Maximum frequency to use in the IRF calculation, the default is
%         the maximum frequency from the BEM data
%
% Returns
% -------
%     hydro : struct
%         Structure of hydro data with radiation IRF
%

p = waitbar(0,'Calculating radiation IRFs...');  % Progress bar

% Set defaults if empty
if isempty(tEnd)==1;  tEnd = 100;           end
if isempty(nDt)==1;    nDt = 1001;            end
if isempty(nDw)==1;    nDw = 1001;            end
if isempty(wMin)==1;  wMin = min(hydro.w);  end
if isempty(wMax)==1;  wMax = max(hydro.w);  end

% Interpolate to the given t and w
t = linspace(0,tEnd,nDt);
w = linspace(wMin,wMax,nDw);
N = sum(hydro.dof) * sum(hydro.dof);

% Calculate the impulse response function for radiation
n = 0;
hydro.ra_K = nan(sum(hydro.dof), sum(hydro.dof), length(t));
for i = 1:sum(hydro.dof)
    for j = 1:sum(hydro.dof)
        ra_B = interp1(hydro.w,squeeze(hydro.B(i,j,:)),w);
        hydro.ra_K(i,j,:) = (2/pi)*trapz(w,ra_B.*(cos(w.*t(:)).*w), 2);
        n = n+1;
    end
    waitbar(n/N)
end

% Calculate the infinite frequency added mass
ra_Ainf_temp = zeros(length(hydro.w),1);                                    %Initialize the variable
[~,F] = size(hydro);                                                        %Last data set in
if strcmp(hydro(F).code,'WAMIT')==0
    for i = 1:sum(hydro.dof)
        for j = 1:sum(hydro.dof)
            ra_A            = squeeze(hydro.A(i,j,:));
            ra_K            = squeeze(hydro.ra_K(i,j,:));
            for k = 1:length(hydro.w)                                       %Calculate the infinite frequency added mass at each input frequency
                ra_Ainf_temp(k,1)  = ra_A(k) + (1./hydro.w(k))*trapz(t,ra_K.*sin(hydro.w(k).*t.'));
            end
            hydro.Ainf(i,j) = mean(ra_Ainf_temp);                           %Take the mean across the vector of infinite frequency added mass
        end
    end
end

hydro.ra_t = t;
hydro.ra_w = w;
close(p)

end


function [ output_real ] = remove_mean( input_real )
    output_real = input_real - mean(input_real);
end


function [U,S,output] = lmlra_rnd(size_tens,size_core,options)
%LMLRA_RND Pseudorandom initialization for LMLRA.
%   [U,S] = lmlra_rnd(size_tens,size_core) generates pseudorandom
%   unitary factor matrices U{1}, ..., U{N} of dimensions size_tens(n)-by-
%   size_core(n) and a core tensor of dimensions size_core that can be used
%   to initialize algorithms that compute a low multilinear rank
%   approximation of an N-th order tensor.
%
%   lmlra_rnd(T,size_core) is shorthand for lmlra_rnd(size(T),size_core) if
%   T is real. If T is complex, then by default S and U{n} will be
%   generated as a complex tensor and matrices, respectively (cf. options).
%
%   lmlra_rnd(size_tens,size_core,options) and lmlra_rnd(T,size_core, ...
%   options) may be used to set the following options:
%
%      options.Real =         - The type of random number generator used to
%      [{@randn}|@rand|0]       generate the real part of the core tensor S
%                               and matrices U{n}. If 0, there is no real
%                               part.
%      options.Imag =         - The type of random number generator used to
%      [@randn|@rand|0|...      generate the imaginary part of the core
%       {'auto'}]               tensor S and matrices U{n}. If 0, there is
%                               no imaginary part. On 'auto', options.Imag
%                               is 0 unless the first argument is a complex
%                               tensor T, in which case it is equal to
%                               options.Real.
%      options.Unitary = true - Set equal to false to generate the factor
%                               matrices U{n} without converting them to
%                               unitary matrices afterwards.
%
%   See also btd_rnd, cpd_rnd, lmlragen.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)

% Process input.
isSizeVector = any(size(size_tens) == numel(size_tens));
if ~isSizeVector, T = size_tens; size_tens = size(size_tens); end
N = length(size_tens);
if N ~= length(size_core)
    error('lmlra_rnd:size_core', ...
         ['length(size_core) should be equal to ndims(T) or ' ...
          'length(size_tens).']);
end

% Check the options structure.
isfunc = @(f)isa(f,'function_handle');
if nargin < 3, options = struct; end
if ~isfield(options,'Real'), options.Real = @randn; end
if ~isfunc(options.Real), options.Real = @zeros; end
if ~isfield(options,'Imag'), options.Imag = 'auto'; end
if ~isfield(options,'Unitary'), options.Unitary = true; end
if ischar(options.Imag) && strcmpi(options.Imag,'auto')
    if ~isSizeVector && ~isreal(T)
        options.Imag = options.Real;
    else
        options.Imag = 0;
    end
end

% Generate factor matrices and core tensor.
U = arrayfun(@(n)options.Real(size_tens(n),size_core(n)),1:N, ...
        'UniformOutput',0);
if isfunc(options.Imag)
    Ui = arrayfun(@(n)options.Imag(size_tens(n),size_core(n)),1:N, ...
            'UniformOutput',0);
    U = cellfun(@(ur,ui)ur+ui*1i,U,Ui,'UniformOutput',0);
end
if options.Unitary
    for n = 1:N
        if size(U{n},1) >= size(U{n},2), [U{n},~] = qr(U{n},0);
        else [Q,~] = qr(U{n}.',0); U{n} = Q.'; end
    end
end
S = options.Real(size_core(:).');
if isfunc(options.Imag), S = S+1i*options.Imag(size_core(:).'); end
output = struct;


function [pos_dot, vel_dot] = satellite_motion_diff_eq(pos, vel, acc, a, GM, J2, Omegae_dot)

% SYNTAX:
%   [pos_dot, vel_dot] = satellite_motion_diff_eq(pos, vel, acc, a, GM, J2, Omegae_dot);
%
% INPUT:
%   pos = satellite position (XYZ)
%   vel = satellite velocity
%   acc = acceleration due to lunar-solar gravitational perturbation
%   a   = ellipsoid semi-major axis [m]
%   GM  = gravitational constant (mass of Earth) [m^3/s^2]
%   J2  = second zonal harmonic of the geopotential
%   Omegae_dot = angular velocity of the Earth rotation [rad/s]
%                (if provided, an Earth-fixed system will be used)
%
% OUTPUT:
%   pos_dot = differential position
%   vel_dot = differential velocity
%
% DESCRIPTION:
%   Computation of the differential equations of perturbed orbital motion.

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

if (nargin < 6)
    %do the computation in an inertial system
    Omegae_dot = 0;
end

%differential position
pos_dot = vel;

%renaming variables for better readability
%
%position
x = pos(1);
y = pos(2);
z = pos(3);
%velocity
vx = vel(1);
vy = vel(2);
%acceleration (i.e. perturbation)
ax = acc(1);
ay = acc(2);
az = acc(3);

%parameters
r = sqrt(x^2 + y^2 + z^2);
g = -GM/r^3;
h = J2*1.5*(a/r)^2;
k = 5*z^2/r^2;

%differential velocity
vel_dot = zeros(size(vel));
vel_dot(1) = g*x*(1 - h*(k - 1)) + ax + Omegae_dot^2*x + 2*Omegae_dot*vy;
vel_dot(2) = g*y*(1 - h*(k - 1)) + ay + Omegae_dot^2*y - 2*Omegae_dot*vx;
vel_dot(3) = g*z*(1 - h*(k - 3)) + az;


classdef SwerlingIVSqLawD
%%SWERLINGIVSQLAWD This class implements various functions and detections
%                statistics related to the Swerling IV target model when
%                used with a square law detector. All methods are static
%                (the class holds no information and does not need to be
%                instantiated).
%
%Implemented methods are: mean, var, PDF, CDF, PD4Threshold, PD4PFA, rand
%
%Swerling models are given in [1] and in Chapter 11 of [2]. The Swerling IV
%model assumes that the observed power SNR of the target varies in terms of
%a central gamma distribution with shape parameter k=2 and scale parameter
%theta=avgSNR/2, that in a pulse train for detection the target SNR
%fluctuates from pulse to pulse, and that a square law detector is used to
%incoherently integrate multiple pulses The difference from a Swerling III
%model is the target amplitude fluctuations from pulse to pulse.

%The square law detector for N samples is
%y=sum_{i=1}^N r_i^2
%where r_i is the ith real amplitude. The model for the amplitude given a
%target amplitude or power is developed in Chapter 9 of [2]. The squared
%real amplitude is r^2_i=y_{I,i}^2+y_{Q,i}^2 where y_{I,i} and y_{Q,i} are
%the in-phase and quadrature components of the filter output. Define
%Rp=2*sampPowSNR, as in Equation 9.2-34. The sampled power SNR of the
%target in the Swerling IV model is different for each pulse. The sample
%power SNR is sampPowSNR=GammaD.rand(1,2,avgSNR/2) as mentioned in [1]. The
%value y=y_{I,i}+sqrt(-1)*y_{Q,i} conditioned on the sampled power SNR is
%modeled as being distributed circularly complex Gaussian with mean
%sqrt(Rp) and variance 2. (See, Equation 9.3-35a in [2]). The variance
%being 2 simply reflects having normalized the variance on the I and Q
%components each to 1.  As in Equation 10.4-13 of [2], the distribution of
%Y=y/2 conditioned on the target SNR values is noncentral chi-squared with
%a change of variables. The value y conditioned on the target SNR values is
%noncentral chi-squared with nu=2*N degrees of freedom and lambda=N*Rp as
%the noncentrality parameter. The expressions in [1] and [2] are in terms
%of Y and not y, so a transformation has to be performed to make things in
%terms of y.
%
%Under this model, for a given average power SNR value, one can generate a
%random sample from the distribution as
% sample=0;
% for curPulse=1:N
%     sampPowSNR=GammaD.rand(1,2,avgSNR/2);
%     Rp=2*sampPowSNR;%The peak power of the signal from the SNR, Equation
%                 %9.4-8 in [2].
%     A=sqrt(Rp);%Equation 9.4-8, the amplitude of the signal
%     sample=sample+sum(abs(ComplexGaussianD.rand(1,A,2)).^2);
% end
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%[2] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

methods(Static)
function val=mean(avgSNR,N)
%%MEAN Obtain the mean of the distribution of the detection power
%      (normalized to a noise variance of 1) under a Swerling IV model in a
%      square-law detector.
%
%INPUTS:avgSNR The average power signal to noise ratio of the target.
%        N The number of pulses that are to be incoherently added for
%          detection (in a square-law detector). In a Swerling IV model, a
%          different realization of the target power is used across each
%          pulse and the noise corrupting the pulses varies. If this
%          parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: val The value of the mean for the given parameters.
%
%The mean is given in Equation 11.5-20 of [1] where equation 11.4-8 relates
%the parameterization used there to an average squared amplitude. To
%relate the average squared amplitude ABar^2 to avgSNR as used in [2], we
%use avgSNR=ABar^2/2. However, the first moment from that expression has to
%be multiplied by 2, because as seen in Equation 10.4-4 of [1], the
%definition is in terms of a scaled version of the square law detector.
%
%EXAMPLE:
%Here, we validate the mean by generating random samples and comparing the
%computed mean to the sample mean.
% avgSNR=2;
% N=4;
% numSamples=1e5;
% meanVal=SwerlingIVSqLawD.mean(avgSNR,N)
% meanSampVal=mean(SwerlingIVSqLawD.rand([numSamples,1],avgSNR,N))
%One will see that both mean values are about 24.
%
%REFERENCES:
%[1] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%[2] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<2||isempty(N))
        N=1;
    end

    val=2*N*(1+avgSNR);
end

function val=var(avgSNR,N)
%%VAR Obtain the variance of the distribution of the detection power
%     (normalized to a noise variance of 1) under a Swerling IV model in a
%     square-law detector.
%
%INPUTS:avgSNR The average power signal to noise ratio of the target.
%        N The number of pulses that are to be incoherently added for
%          detection (in a square-law detector). In a Swerling IV model, a
%          different realization of the target power is used across each
%          pulse and the noise corrupting the pulses varies. If this
%          parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: val The value of the variance for the given parameters.
%
%The variance is given in Equation 11.5-22b of [1] where equation 11.4-8
%relates the parameterization used there to an average squared amplitude.
%To relate the average squared amplitude ABar^2 to avgSNR as used in [2],
%we use avgSNR=ABar^2/2. However, the variance from that expression has to
%be multiplied by 4, because as seen in Equation 10.4-4 of [1], the
%definition in is in terms of a scaled version of the square law detector.'
%
%EXAMPLE:
%Here, we validate the variance by generating random samples and comparing
%the computed variance to the sample variance.
% avgSNR=2;
% N=4;
% numSamples=1e5;
% varVal=SwerlingIVSqLawD.var(avgSNR,N)
% varSampVal=var(SwerlingIVSqLawD.rand([numSamples,1],avgSNR,N))
%One will see that both variance values are about 112.
%
%REFERENCES:
%[1] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%[2] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<2||isempty(N))
        N=1;
    end

    val=4*N*(1+avgSNR.*(2+avgSNR/2));
end

function val=PDF(x,avgSNR,N)
%%PDF Evaluate the scalar probability density function (PDF) of the
%     distribution of the detection power (normalized to a noise variance
%     of 1) under a Swerling IV model in a square-law detector.
%
%INPUTS: x The point or points at which the PDF should be evaluated. v>=0
%          for nonzero PDF values.
%   avgSNR The average power signal to noise ratio of the target.
%        N The number of pulses that are to be incoherently added for
%          detection (in a square-law detector). In a Swerling IV model, a
%          different realization of the target power is used across each
%          pulse and the noise corrupting the pulses varies. If this
%          parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: val The values of the PDF evaluated at the given points.
%
%The PDF is given in Equation 11.5-14 where equation 11.4-8 relates the
%parameterization used there to an average squared amplitude. To relate the
%average squared amplitude ABar^2 to avgSNR as used in [2], we use
%avgSNR=ABar^2/2. However the PDF is derived is based on a scaled
%square-law detector, as in Equation 10.4-4 in [1]. Thus, in implementing
%the PDF, the scaling is removed.
%
%EXAMPLE:
%Here, we validate the PDF by generating random samples and comparing the
%PDF plot with a histogram of the random samples.
% avgSNR=2;
% N=4;
% numSamples=10e3;
%
% figure(1)
% clf
% histogram(SwerlingIVSqLawD.rand([numSamples,1],avgSNR,N),'Normalization','pdf')
% hold on
% numPoints=1000;
% x=linspace(0,80,numPoints);
% vals=SwerlingIVSqLawD.PDF(x,avgSNR,N);
% plot(x,vals,'linewidth',2)
% h1=xlabel('x');
% h2=ylabel('PDF(x)');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%One will see that the histogram matches well with the plot.
%
%REFERENCES:
%[1] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%[2] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
        N=1;
    end

    %Initial change of variable to undo the scaling from Equation 10.4-4 in
    %[1].
    x=x/2;

    xBar2=avgSNR/2;
    val=zeros(size(x));
    numEls=numel(x);

    for curEl=1:numEls
        v=x(curEl);

        if(v>0)
            val(curEl)=exp((N-1)*log(v)-v/(1+xBar2)-2*N*log(1+xBar2)-gammaln(N))*hypergeometric1F1(-N,N,-xBar2/(1+xBar2)*v);
        elseif(v==0&&N==1)
            val(curEl)=1/((1+xBar2)^(2*N)*factorial(N-1));
        end
    end

    %Adjust for the change of variable to undo the scaling from Equation
    %10.4-4 in [1].
    val=val/2;
end

function val=CDF(v,avgSNR,N)
%%CDF Evaluate the scalar cumulative distribution function (CDF) of the
%     distribution of the detection power (normalized to a noise variance
%     of 1) under a Swerling IV model in a square-law detector.
%
%INPUTS: v The point or points at which the CDF should be evaluated. v>0
%          for nonzero CDF values.
%   avgSNR The average power signal to noise ratio of the target.
%        N The number of pulses that are to be incoherently added for
%          detection (in a square-law detector). In a Swerling IV model, a
%          different realization of the target power is used across each
%          pulse and the noise corrupting the pulses varies. If this
%          parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: val The values of the CDF evaluated at the given points.
%
%The derivation of the detection probability in [1] is as 1-the value of
%the CDF. Thus, this function just evaluated
%1-SwerlingIVSqLawD.PD4Threshold(avgSNR,v,N).
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
        N=1;
    end

    val=1-SwerlingIVSqLawD.PD4Threshold(avgSNR,v,N);
    val(v<=0)=0;
end

function PD=PD4Threshold(avgSNR,thresh,N)
%%PD4THRESHOLD Determine the detection probability of a Swerling IV target
%           given its signal to noise ratio and the value of the
%           detection threshold, assuming that a square-law detector is
%           used. The noise in each sample is assumed to be Gaussian with
%           variance 1 (normalized noise power).
%
%INPUTS: avgSNR A vector or matrix of average power signal to noise ratios
%               of the target at which one wishes to evaluate the detection
%               probability.
%        thresh The scalar normalized detection threshold to use. This is
%               the threshold to use if the noise variance is 1.
%             N The number of pulses that are to be incoherently added for
%               detection (in a square-law detector). In a Swerling IV
%               model, a different realization of the target power is used
%               across each pulse and the noise corrupting the pulses
%               varies. If this parameter is omitted or an empty matrix is
%               passed, N=1 is used.
%
%OUTPUTS: PD The detection probability of the target.
%
%This function implements the sum in Equations 11.5-19 in [1]. However, the
%expression used is derived based on a scaled square-law detector, as in
%Equation 10.4-4. Thus, the threshold in this function is scaled
%appropriately.
%
%EXAMPLE:
%Here, we validate the results by comparing the PD from this function to
%the PD computed using random samples.
% avgSNR=2;
% thresh=30;
% N=4;
% PD=SwerlingIVSqLawD.PD4Threshold(avgSNR,thresh,N)
% numSamples=1e5;
% PDSamp=mean(SwerlingIVSqLawD.rand([numSamples,1],avgSNR,N)>=thresh)
%One will see that both PD values are about 0.254.
%
%REFERENCES:
%[1] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
       N=1;
    end

    thresh=thresh/2;

    R=2*avgSNR;

    logR4=log(R/4);
    logCoeff=gammaln(N+1)-N*log(1+R/4);
    sumVal=0;
    for k=0:N
        curTerm=exp(logCoeff+k*logR4+log(gammainc(thresh/(1+R/4),k+N))-gammaln(k+1)-gammaln(N-k+1));

        sumVal=sumVal+curTerm;
    end

    PD=1-sumVal;
end

function PD=PD4PFA(avgSNR,PFA,N)
%%PD4PFA Determine the detection probability of a Swerling IV target given
%        its signal to noise ratio and the probability of false alarm
%        implied by a particular unspecified detection threshold, assuming
%        that a square law detector is used.
%
%INPUTS: avgSNR A vector or matrix of average power signal to noise ratios
%               of the target at which one wishes to evaluate the detection
%               probability.
%           PFA The probability of false alarm, 0<=PFA<1.
%             N The number of pulses that are to be incoherently added for
%               detection (in a square-law detector). In a Swerling IV
%               model, a different realization of the target power is used
%               across each pulse and the noise corrupting the pulses
%               varies. If this parameter is omitted or an empty matrix is
%               passed, N=1 is used.
%
%OUTPUTS: PD The detection probability of the target, 0<=PD<=1.
%
%This function just calls PFA2SquareLawThreshold to convert the false alarm
%rate into a normalized detection threshold after which it calls
%SwerlingIVSqLawD.PD4Threshold.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
       N=1;
    end

    thresh=PFA2SquareLawThreshold(PFA,N);
    PD=SwerlingIVSqLawD.PD4Threshold(avgSNR,thresh,N);
end

function vals=rand(NDims,avgSNR,N)
%%RAND Generate random variables representing a target whose sampled signal
%      to average noise power ratio is generated according to a Swerling IV
%      model detected by a square-law detector.
%
%INPUTS: NDims If NDims is a scalar, then rand returns an NDimsXNDims
%          matrix of random variables. If NDims=[M,N1] is a two-element row
%          vector, then rand returns an MXN1 matrix of random variables.
%   avgSNR The average power signal to noise ratio of the target.
%        N The number of pulses that are to be incoherently added for
%          detection (in a square-law detector). In a Swerling IV model, a
%          different realization of the target power is used across each
%          pulse and the noise corrupting the pulses varies. If this
%          parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: vals A matrix whose dimensions are determined by N of the
%              generated random variables.
%
%Following the model of [1], a sample from the central gamma distribution
%is performed to obtain the complex signal to noise ratio of every single
%one of the N pulses. Then, the square law detector output conditioned on
%the signal to noise ratio is generated directly using NonFlucSqLawD.rand
%for each pulse and the values are added.
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
        N=1;
    end

    if(isscalar(NDims))
        dims=[NDims, NDims];
    else
        dims=NDims;
    end

    vals=zeros(dims);

    numEls=numel(vals);
    for curEl=1:numEls
        for curPulse=1:N
            %Equation I.2 for  the input signal-to-noise power ratio. It is
            %different for each sample in the pulse train in the Swerling
            %IV model.

            curSNR=GammaD.rand(1,2,avgSNR/2);
            vals(curEl)=vals(curEl)+NonFlucSqLawD.rand(1,curSNR,1);
        end
    end
end
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 status = test_linkwitz_riley(modus)
%TEST_LINKWITZ_RILEY tests the design of the Linkwitz-Riley Filters
%
%   Usage: status = test_linkwitz_riley(modus)
%
%   Input parameters:
%       modus   - 0: numerical
%                 1: visual
%
%   Output parameters:
%       status  - true or false

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

status = false;

%% ===== Checking of input  parameters ===================================
nargmin = 1;
nargmax = 1;
narginchk(nargmin,nargmax);

%% ===== Main ============================================================
fs = 44100;  % sampling frequency in Hz
fc = 1000;  % crossover frequency in Hz
f = logspace(-3,log10(fs/2),10000);  % frequeny-axis in Hz
omega = 2*pi*f;  % angular frequency

ftype = {'low' 'high' 'all'};  % filter types
Hs = zeros(3,length(f));  % s-domain frequency spectra
Hz = Hs;  % z-domain frequency spectra

for Nlr = [2 4 12]  % filter orders

  for tdx = 1:3
    % Laplace-Domain
    [zs,ps,ks] = linkwitz_riley(Nlr,2*pi*fc,ftype{tdx},'s');
    [soss,gs] = zp2sos(zs,ps,ks,'down','none');

    s = [-omega.^2; 1i*omega];
    s(3,:) = 1;

    if isoctave && strcmp(ftype{tdx}, 'low')
      % WORKAROUND for Octave bug (https://savannah.gnu.org/bugs/?51936)
      Hs(tdx,:) = gs.*prod( (soss(:,3:-1:1)*s)./(soss(:,4:6)*s),1);
    else
      Hs(tdx,:) = gs.*prod( (soss(:,1:3)*s)./(soss(:,4:6)*s),1);
    end

    % z-Domain
    [zz,pz,kz] = linkwitz_riley(Nlr,fc./fs*2,ftype{tdx},'z');
    [sosz,gz] = zp2sos(zz,pz,kz,'down','none');

    z = [exp(0.*omega./fs); exp(-1j.*omega./fs); exp(-2j.*omega./fs)];
    Hz(tdx,:) = gz.*prod( (sosz(:,1:3)*z)./(sosz(:,4:6)*z),1);
  end

  % plotting
  if modus
    figure;
    subplot(2,2,1);
    plot_amp(f, Hs);
    title(sprintf('Amplitude Spectrum, s-Domain, order=%d', Nlr));
    subplot(2,2,2);
    plot_phase(f, Hs);
    title(sprintf('Phase Spectrum, s-Domain, order=%d', Nlr));
    subplot(2,2,3);
    plot_amp(f, Hz);
    title(sprintf('Amplitude Spectrum, z-Domain, order=%d', Nlr));
    subplot(2,2,4);
    plot_phase(f, Hz);
    title(sprintf('Phase Spectrum, z-Domain, order=%d', Nlr));
  end
end

status = true;

end

function plot_amp(f, H)
semilogx( ...
  f, db(H(1,:)), 'b', ...
  f, db(H(2,:)), 'r', ...
  f, db(H(1,:)+H(2,:)), 'g.', ...
  f, db(H(3,:)), 'k--' ...
  );
legend('Lowpass', 'Highpass', 'Lowpass + Highpass', 'Allpass', 'Location', 'southwest');
xlabel('f / Hz');
ylabel('20 lg |H| / dB')
xlim([f(1), f(end)]);
ylim([-90, 10]);
end

function plot_phase(f, H)
semilogx( ...
  f, unwrap(angle(H(1,:))), 'b', ...
  f, unwrap(angle(H(2,:))), 'r', ...
  f, unwrap(angle(H(1,:)+H(2,:))), 'g.', ...
  f, unwrap(angle(H(3,:))), 'k--' ...
  );
legend('Lowpass', 'Highpass', 'Lowpass + Highpass', 'Allpass', 'Location', 'southwest');
xlabel('f / Hz');
ylabel('angle(H) / rad')
xlim([f(1), f(end)]);
end


function ydata = tsne_p(P, labels, no_dims)
%TSNE_P Performs symmetric t-SNE on affinity matrix P
%
%   mappedX = tsne_p(P, labels, no_dims)
%
% The function performs symmetric t-SNE on pairwise similarity matrix P
% to create a low-dimensional map of no_dims dimensions (default = 2).
% The matrix P is assumed to be symmetric, sum up to 1, and have zeros
% on the diagonal.
% The labels of the data are not used by t-SNE itself, however, they
% are used to color intermediate plots. Please provide an empty labels
% matrix [] if you don't want to plot results during the optimization.
% The low-dimensional data representation is returned in mappedX.
%
%
% (C) Laurens van der Maaten, 2010
% University of California, San Diego

    if ~exist('labels', 'var')
        labels = [];
    end
    if ~exist('no_dims', 'var') || isempty(no_dims)
        no_dims = 2;
    end

    % First check whether we already have an initial solution
    if numel(no_dims) > 1
        initial_solution = true;
        ydata = no_dims;
        no_dims = size(ydata, 2);
    else
        initial_solution = false;
    end

    % Initialize some variables
    n = size(P, 1);                                     % number of instances
    momentum = 0.5;                                     % initial momentum
    final_momentum = 0.8;                               % value to which momentum is changed
    mom_switch_iter = 250;                              % iteration at which momentum is changed
    stop_lying_iter = 100;                              % iteration at which lying about P-values is stopped
    max_iter = 1000;                                    % maximum number of iterations
    epsilon = 500;                                      % initial learning rate
    min_gain = .01;                                     % minimum gain for delta-bar-delta

    % Make sure P-vals are set properly
    P(1:n + 1:end) = 0;                                 % set diagonal to zero
    P = 0.5 * (P + P');                                 % symmetrize P-values
    P = max(P ./ sum(P(:)), realmin);                   % make sure P-values sum to one
    const = sum(P(:) .* log(P(:)));                     % constant in KL divergence
    if ~initial_solution
        P = P * 4;                                      % lie about the P-vals to find better local minima
    end

    % Initialize the solution
    if ~initial_solution
        ydata = .0001 * randn(n, no_dims);
    end
    y_incs  = zeros(size(ydata));
    gains = ones(size(ydata));

    % Run the iterations
    for iter=1:max_iter

        % Compute joint probability that point i and j are neighbors
        sum_ydata = sum(ydata .^ 2, 2);
        num = 1 ./ (1 + bsxfun(@plus, sum_ydata, bsxfun(@plus, sum_ydata', -2 * (ydata * ydata')))); % Student-t distribution
        num(1:n+1:end) = 0;                                                 % set diagonal to zero
        Q = max(num ./ sum(num(:)), realmin);                               % normalize to get probabilities

        % Compute the gradients (faster implementation)
        L = (P - Q) .* num;
        y_grads = 4 * (diag(sum(L, 1)) - L) * ydata;

        % Update the solution
        gains = (gains + .2) .* (sign(y_grads) ~= sign(y_incs)) ...         % note that the y_grads are actually -y_grads
              + (gains * .8) .* (sign(y_grads) == sign(y_incs));
        gains(gains < min_gain) = min_gain;
        y_incs = momentum * y_incs - epsilon * (gains .* y_grads);
        ydata = ydata + y_incs;
        ydata = bsxfun(@minus, ydata, mean(ydata, 1));

        % Update the momentum if necessary
        if iter == mom_switch_iter
            momentum = final_momentum;
        end
        if iter == stop_lying_iter && ~initial_solution
            P = P ./ 4;
        end

        % Print out progress
        if ~rem(iter, 10)
            cost = const - sum(P(:) .* log(Q(:)));
            disp(['Iteration ' num2str(iter) ': error is ' num2str(cost)]);
        end

        % Display scatter plot (maximally first three dimensions)
        if ~rem(iter, 10) && ~isempty(labels)
            if no_dims == 1
                scatter(ydata, ydata, 9, labels, 'filled');
            elseif no_dims == 2
                scatter(ydata(:,1), ydata(:,2), 9, labels, 'filled');
            else
                scatter3(ydata(:,1), ydata(:,2), ydata(:,3), 40, labels, 'filled');
            end
            axis tight
            axis off
            drawnow
        end
    end


function [c, s] = foopsi(y, g, lam, solver)
%% Infer the most likely discretized spike train underlying an AR(1) fluorescence trace
% Solves the sparse non-negative deconvolution problem
%  min 1/2|c-y|^2 + lam |s|_1 subject to s_t = c_t-g c_{t-1} >= 0

%% inputs:
%   y:  T*1 vector, One dimensional array containing the fluorescence intensities
%withone entry per time-bin.
%   g:  scalar, Parameter of the AR(1) process that models the fluorescence ...
%impulse response.
%   lam:  scalar, sparsity penalty parameter lambda.
%   solver: string, optimization solver

%% outputs
%   c: T*1 vector, the inferred denoised fluorescence signal at each time-bin.
%   s: T*1 vector, discetized deconvolved neural activity (spikes)

%% Authors: Pengcheng Zhou, Carnegie Mellon University, 2016
% ported from the Python implementation from Johannes Friedrich

%% References
% Friedrich J et.al., NIPS 2016, Fast Active Set Method for Online Spike Inference from Calcium Imaging

%% initialization
y = reshape(y, [], 1);
T = length(y);
if ~exist('g', 'var') || isempty(g);   g = estimate_time_constant(y, 2); end

if ~exist('lam', 'var') || isempty(lam);   lam = 0; end
if ~exist('solver', 'var') || isempty(smin);   solver = 'SDPT3'; end

% construct the deconvolution matrix (s=G*c)
len_g = length(g);
g = reshape(g, 1, []);
indc = bsxfun(@minus, (1:T)', 0:len_g);
indr = repmat((1:T)', [1, len_g+1]);
v = ones(T,1)*[1,-g];
ind = (indc>0);
G = sparse(indr(ind), indc(ind), v(ind), T, T);

%% run optimization
% cvx_solver(solver);

cvx_begin quiet
variable c(T)
minimize(0.5*(c-y)'*(c-y) + lam*(1-sum(g))*norm(c,1))
subject to
G*c >=0;
cvx_end

s = reshape(G*c, 1, []);
s(1) = 0;
c = reshape(c,1, []);


function r = polya_sample(a,n)
% POLYA_SAMPLE      Sample from Dirichlet-multinomial (Polya) distribution.
% POLYA_SAMPLE(a,n) returns a matrix of histograms.
% If A is a row, the histograms are the rows, others they are the columns.
% N is a vector whose length is the number of histograms.
% N(i) will be the total count in histogram i.

row = (rows(a) == 1);

a = a(:);
p = dirichlet_sample(a,length(n));
r = zeros(size(p));
for i = 1:length(n)
  r(:,i) = sample_hist(p(:,i),n(i));
end
if row
  r = r';
end


%THIS FUMCTION ILLUSTRATES HOW TO USE THE USER-DEFINED ALGORITHM
function [AH,XH] = user_alg1(Y,r,X)
%
% The example of implementing the user-defined algorithm (this is the regularized Fixed-Point algorithm based)
%
% INPUTS:
% Y - mixed signals (matrix of size [m by T])
% r - number of estimated signals
% X - true source signals
%
% OUTPUTS
% AH - estimated mixing matrix (matrix of size [m by r])
% XH - estimated source signals (matrix of size [r by T])
%
% #########################################################################
% Initialization
[m,T]=size(Y);
Y(Y <=0) = eps; % this enforces the positive value in the data
AH=rand(m,r);
XH=rand(r,T);

IterNo = 1000; % number of alternating steps

% Iterations
for k = 1:IterNo

    alpha_reg = 20*exp(-k/10); % regularization parameter
    XH = max(1E6*eps,pinv(AH'*AH +  alpha_reg)*AH'*Y);
    AH = max(1E6*eps, Y*XH'*pinv(XH*XH' + alpha_reg));
    AH = AH*diag(1./(sum(AH,1) + eps));

end


x=@(s,t) 3*cos(s);
y=@(s,t) 2*sin(s);
z=@(s,t) t;
ezmesh(x,y,z)


function Omega = generateQ(m)

% generate an orthonormal matrix.

G     = randn(m);
[Q,R] = qr(G);
% normalize to positive entry in the diagonal
In    = diag(sign(diag(R)));
Omega = Q  * In;


function sigma = dtiComputeImageNoise(dwRaw, bvals, brainMask, noiseCalcMethod)
% Compute image noise from a 4-d DWI image
%
% sigma = dtiComputeImageNoise(dwRaw, bvals, brainMask, [noiseCalcMethod])
%
% Takes in dwi data and returns sigma, the image noise
%
% dwRaw    - Nifti structure of the dwi data
% bvals    - A 1xN vector of bvalues for each volume in dwRaw
% brainMask- A 3-D matrix of binary values  denoting which voxels are
%            insided the brain. This is only necesary for noiseCalcMethod =
%            'b0'
% noiseCalcMethod- Whether to calculate the noise based on the corner of
%                  the image (default) or based on the standard deviation
%                  of the b=0 images ('b0').

if ~exist('noiseCalcMethod','var') || isempty(noiseCalcMethod)
    noiseCalcMethod = 'corner';
end

if strcmp('corner', noiseCalcMethod)

    % According to Henkelman (1985), the expected signal variance (sigma) can
    % be computed as 1.5267 * SD of the background (thermal) noise.
    sz = size(dwRaw.data);
    x  = 10;
    y  = 10;
    z  = round(sz(3)/2);

    [x,y,z,s] = ndgrid(x-5:x+5, y-5:y:5, z-5:z+5, 1:sz(4));
    noiseInds = sub2ind(sz, x(:), y(:), z(:), s(:));
    sigma     = 1.5267 * std(double(dwRaw.data(noiseInds)));

elseif strcmp('b0', noiseCalcMethod)

    % Number of volumes in the dw dataset
    numVols   = size(dwRaw.data,4);
    % Get brainmask indices
    brainInds = find(brainMask);
    % preallocate a 2d array (with a 2nd dimension that is a singleton). The
    % first dimension is the number of volumes and the 3rd is each voxel
    % (within the brain mask).
    data      = zeros(numVols,1,length(brainInds));
    % Loop over the volumes and assign the voxels within the brain mask to data
    for ii=1:numVols
        tmp = double(dwRaw.data(:,:,:,ii));
        data(ii,1,:) = tmp(brainInds);
    end

    % Find which volumes ar b=0
    b0inds = find(bvals ==  0);
    n = length(b0inds);
    % Pull out the b=0 volumes
    dataB0 = squeeze(data(b0inds,1,:));
    % Calculate the median of the standard deviation. We do not think that
    % this needs to be rescaled. Henkelman et al. (1985) suggest that this
    % aproaches the true noise as the signal increases.
    sigma = median(std(dataB0,0,1));

    % std of a sample underestimates sigma (see http://nbviewer.ipython.org/4287207/)
    % This can be very big for small n (e.g., 20% for n=2)
    % We can compute the underestimation bias:
    bias = sigma * (1 - sqrt(2 / (n-1)) * (gamma(n / 2) / gamma((n-1) / 2)));

    % and correct for it:
    sigma = sigma + bias;
end


function polyhedra(varargin)
%POLYHEDRA Index of classical polyhedral meshes
%
%   Polyhedra are specific meshes, with additional assumptions:
%   * the set of faces is assumed to enclose a single 3D domain
%   * each face has a neighbor face for each edge
%   * some functions also assume that normals of all faces point outwards
%
%   Most polyhedron creation functions follow the patterns:
%   * [V, F] = createXXX();     % returns vertex and face arrays
%   * [V, E, F] = createXXX();  % returns also edge array
%   * M = createXXX();          % return a data structure with 'vertices',
%                               % 'edges' and 'faces' fields.
%
%   Example
%   % create a soccer ball mesh and display it
%   [n, f] = createSoccerBall;
%   drawMesh(n, f, 'faceColor', 'g', 'linewidth', 2);
%   axis equal;
%
%   See also
%   meshes3d
%   createCube, createCubeOctahedron, createIcosahedron, createOctahedron
%   createRhombododecahedron, createTetrahedron, createTetrakaidecahedron
%   createDodecahedron, createSoccerBall, createMengerSponge
%   steinerPolytope, minConvexHull
%   polyhedronNormalAngle, polyhedronMeanBreadth
%

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

% HISTORY


function kern = rbfardKernParamInit(kern)

% RBFARDKERNPARAMINIT RBFARD kernel parameter initialisation.
% The automatic relevance determination version of the radial basis
% function kernel (RBFARD) is a very smooth non-linear kernel and is a
% popular choice for generic use.
%
% k(x_i, x_j) = sigma2 * exp(-gamma/2 *(x_i - x_j)'*A*(x_i - x_j))
%
% The parameters are sigma2, the process variance (kern.variance), the
% diagonal matrix of input scales (kern.inputScales, constrained to be
% between zero and one) and gamma, the inverse width
% (kern.inverseWidth). The inverse width controls how wide the basis
% functions are, the larger gamma, the smaller the basis functions
% are.
%
% SEEALSO : rbfKernParamInit
%
% FORMAT
% DESC initialises the automatic relevance determination radial basis function
%  kernel structure with some default parameters.
% ARG kern : the kernel structure which requires initialisation.
% RETURN kern : the kernel structure with the default parameters placed in.
%
% SEEALSO : kernCreate, kernParamInit
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% KERN

% This parameter is restricted positive.
kern.inverseWidth = 2/kern.inputDimension;
kern.variance = 1;
% These parameters are restricted to lie between 0 and 1.
kern.inputScales = 0.999*ones(1, kern.inputDimension);
kern.nParams = 2 + kern.inputDimension;

kern.transforms(1).index = [1 2];
kern.transforms(1).type = optimiDefaultConstraint('positive');
kern.transforms(2).index = [3:kern.nParams];
kern.transforms(2).type = optimiDefaultConstraint('zeroone');

kern.isStationary = true;


function [MU,k]=vonMisesStat(T)

% function [MU,k]=vonMisesStat(T)
% ------------------------------------------------------------------------
%
%
% ------------------------------------------------------------------------

%%
MU=angle(mean(exp(1i.*T)));
Rsq=mean(cos(T)).^2+mean(sin(T)).^2;
R=sqrt(Rsq);

p=2;
ki=R.*(p-Rsq)./(1-Rsq);
diffTol=ki/1000;
qIter=1;
while 1
    kn=ki;
    A=besseli(p/2,ki)./ besseli((p/2)-1,ki);
    ki=ki-((A-R)./(1-A^2-((p-1)/ki)*A));
    if abs(kn-ki)<=diffTol
        break
    end
    qIter=qIter+1;
end
k=ki;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


close all;
clear;
clc;

%% load dataset
%load('../dataset/ORL_Face_img.mat');
%load('../dataset/Brodatz_texture_img_small_set.mat');
%load('../dataset/AR_Face_img_27x20.mat');
%load('../dataset/AR_Face_img_60x43.mat');
load('../dataset/USPS.mat');
%load('../dataset/MNIST.mat');
%load('../dataset/COIL20.mat');
%load('../dataset/COIL100.mat');

%% reduce dataset for a quick test if necessary
max_class_num = 10;
max_train_samples = 2;
max_test_samples = 15;
[TrainSet, TestSet, train_num, test_num, class_num] = reduce_dataset(TrainSet, TestSet, max_class_num, max_train_samples, max_test_samples);

%% set paramters
eigenface_flag = true;
eigenface_dim = floor(train_num/2); % example
dim = size(TrainSet.X, 1);
if eigenface_dim > dim
    eigenface_dim = dim;
end

%% normalize dataset
[TrainSet_normalized.X, TrainSet_normalized.y] = data_normalization(TrainSet.X, TrainSet.y, 'std');
[TestSet_normalized.X, TestSet_normalized.y] = data_normalization(TestSet.X, TestSet.y, 'std');

%% SVM
options.verbose = true;
options.eigenface = eigenface_flag;
options.eigenface_dim = eigenface_dim;
accuracy_svm = svm_classifier(TrainSet, TestSet, train_num, test_num, class_num, options);
fprintf('# SVM: Accuracy = %5.5f\n', accuracy_svm);

%% LSR
lambda = 0.001;
options.verbose = true;
[accuracy_lsr, ~, ~] = lsr(TrainSet, TestSet, train_num, test_num, class_num, lambda, options);
fprintf('# LSR: Accuracy = %5.5f\n', accuracy_lsr);

%% LRC
clear options;
options.verbose = true;
accuracy_lrc = lrc(TrainSet_normalized, TestSet_normalized, test_num, class_num, options);
fprintf('# LRC: Accuracy = %5.5f\n', accuracy_lrc);

%% LDRC
clear options;
options.verbose = true;
accuracy_ldrc = ldrc(TrainSet_normalized, TestSet_normalized, train_num, test_num, class_num, eigenface_dim, options);
fprintf('# LDRC: Accuracy = %5.5f\n', accuracy_ldrc);

%% LCDRC
clear options;
options.verbose = true;
accuracy_lcdrc = lcdrc(TrainSet_normalized, TestSet_normalized, train_num, test_num, class_num, eigenface_dim, options);
fprintf('# LCDRC: Accuracy = %5.5f\n', accuracy_lcdrc);

%% display accuracy
fprintf('\n\n## Summary of results\n\n')
fprintf('# SVM: Accuracy = %5.5f\n', accuracy_svm);
fprintf('# LSR: Accuracy = %5.5f\n', accuracy_lsr);
fprintf('# LRC: Accuracy = %5.5f\n', accuracy_lrc);
fprintf('# LDRC: Accuracy = %5.5f\n', accuracy_ldrc);
fprintf('# LCDRC: Accuracy = %5.5f\n', accuracy_lcdrc);


function varargout = quad2d(varargin)
%QUAD2D  Complete definite integral of SPHEREFUN.
%   I = QUAD2D( F ), returns the definite integral of a SPHEREFUN integrated
%   over its domain of definition.
%
%   I = QUAD2D(F, a, b, c, d), returns the definite integral of a SPHEREFUN.
%   Integrated over the domangle [a b] x [c d].
%
% See also SPHEREFUN/INTEGRAL2, SPHEREFUN/SUM2, SPHEREFUN/INTEGRAL.

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

[varargout{1:nargout}] = quad2d@separableApprox(varargin{:});

end


function [ftlb] = kJ2ftlb(kJ)
% Convert energy or work from kilojoules to foot-pounds.
% Chad A. Greene 2012
ftlb = kJ*737.56217557;


function mappedX = cfa(X, no_dims, no_analyzers, no_iterations)
%CFA Performs manifold charting on dataset X
%
%   mappedX = cfa(X, no_dims, no_analyzers, no_iterations)
%
% Performs manifold charting on dataset X to reduce its dimensionality to
% no_dims dimensions. The variable no_analyzers determines the number of
% local factor analyzers that is used in the mixture of factor analyzers
% (default = 40). The variable no_iterations sets the number of
% iterations that is employed in the EM algorithm (default = 200).
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    if ~exist('no_dims', 'var')
        no_dims = 2;
    end
    if ~exist('no_analyzers', 'var')
        no_analyzers = 40;
    end
    if ~exist('no_iterations', 'var')
        no_iterations = 200;
    end

    % Make sure data is zero-mean, unit-variance
    X = X - repmat(mean(X, 1), [size(X, 1) 1]);
    X = X ./ repmat(var(X, 1), [size(X, 1) 1]);

    % Initialize some parameters
    min_var = 1e-5;                     % minimum STD of Gaussians
    X = X';
    [D n] = size(X);
    c = no_analyzers;
    d = no_dims;

    % Randomly initialize the parameters of the CFA model
    SigmaN = repmat(eye(d), [1 1 n]);
    Z = rand(d, n) * .1;
    Pi = repmat(1 / c, [1 c]);
    Kappa = rand(d, c) * .1;
    Mu = randn(D, c) * .1;
    SigmaC = repmat(eye(d), [1 1 c]);
    Lambda = rand(D, d, c) * .1;
    Psi = zeros(D, D, c);
    for j=1:c
        tmp = zeros(D, D);
        tmp(1:size(tmp, 2) + 1:end) = rand(D, 1) * .05;
        Psi(:,:,j) = tmp + tmp';
    end

    % Perform the EM algorithm for the optimization
    iter = 0;
    while iter < no_iterations

        % E-step
        % ====================================================

        % Do some precomputations for speed
        invPsi = zeros(size(Psi));
        detSigmaC = zeros(1, c);
        detPsi = zeros(1, c);
        logPi = zeros(1, c);
        for j=1:c
            invPsi(:,:,j) = inv(Psi(:,:,j) + eye(D));
            detSigmaC(j) = det(SigmaC(:,:,j));
            detPsi(j) = det(Psi(:,:,j));
            logPi(j) = log(Pi(j));
        end

        % Compute matrices Epsilon, Vc, and m
        Eps = zeros(n, c);
        Vc = zeros(d, d, c);
        m = zeros(d, c);
        const = ((D + d) / 2) * log(2 * pi);
        for j=1:c

            % Precomputations
            Xnc = X - repmat(Mu(:,j), [1 n]);
            Znc = Z - repmat(Kappa(:,j), [1 n]);
            tmpProd = Lambda(:,:,j)' * invPsi(:,:,j) * Lambda(:,:,j);

            % Compute Epsilon
            for i=1:n
                normX = (Xnc(:,i) - Lambda(:,:,j) * Znc(:,i));
                Eps(i, j) = -logPi(j) + const ...
                            + (.5 * log(detSigmaC(j))) + (.5 * detPsi(j)) ...
                            + (.5 * trace(SigmaC(:,:,j) * (SigmaN(:,:,i) + Znc(:,i) * Znc(:,i)'))) ...
                            + (.5 * trace(SigmaN(:,:,i) * tmpProd)) ...
                            + (.5 * normX' * invPsi(:,:,j) * normX);
            end

            % Compute Vc and m
            Vc(:,:,j) = inv(SigmaC(:,:,j) + eps * eye(d)) + tmpProd;
            m(:,j) = Kappa(:,j) + (Vc(:,:,j) \ Lambda(:,:,j)') * invPsi(:,:,j) * mean(Xnc, 2);
        end

        % Update estimate of Q
        Q = (repmat(sum(exp(-Eps), 2), [1 c]) .^ -1) .* exp(-Eps);

        % Update estimate of SigmaN
        for i=1:n
            tmp = zeros(d, d);
            for j=1:c
                tmp = tmp + Q(i, j) * Vc(:,:,j);
            end

            % Compute covariance matrix
            SigmaN(:,:,i) = inv(tmp + eps * eye(d));           % code above gave us inv(SigmaN)
        end

        % Update estimate of Z (= mappedX)
        for i=1:n
            tmp = zeros(d, 1);
            for j=1:c
                tmp = tmp + (Q(i, j) * Vc(:,:,j) * m(:,j));
            end
            Z(:,i) = SigmaN(:,:,i) * tmp;
        end

        % M-step
        % ====================================================

        % Update estimate of Pi
        Pi = sum(Q, 1) ./ n;

        % Update estimate of Mu and Kappa
        for j=1:c
            tmpQ = Q(:,c) ./ sum(Q(:,c));
            Mu(:,c) = sum(repmat(tmpQ', [D 1]) .* X, 2);
            Kappa(:,c) = sum(repmat(tmpQ', [d 1]) .* Z, 2);
        end

        % Update estimate of SigmaC
        for j=1:c
            tmp = 0;
            tmpQ = Q(:,c) ./ sum(Q(:,c));
            for i=1:n
                Znc = Z(:,i) - Kappa(:,j);
                tmp = tmp + (tmpQ(i) * (SigmaN(:,:,i) + Znc * Znc'));
            end

            % Enforce some variance
            tmp(1:size(tmp, 1) + 1:end) = max(min_var, tmp(1:size(tmp, 1) + 1:end));
            SigmaC(:,:,j) = tmp;
        end

        % Update estimate of Lambda
        for j=1:c
            Sc = zeros(D, d);
            tmpQ = Q(:,j) ./ sum(Q(:,j));
            Xnc = X - repmat(Mu(:,j), [1 n]);
            Znc = Z - repmat(Kappa(:,j), [1 n]);
            for i=1:n
                Sc = Sc + tmpQ(i) * (Xnc(:,i) * Znc(:,i)');
            end
            Lambda(:,:,j) = Sc / (SigmaC(:,:,j) + eps * eye(d));
        end

        % Update estimate of Psi
        for j=1:c
            tmpPsi = zeros(D, D);
            tmpQ = Q(:,j) ./ sum(Q(:,j));
            Xnc = X - repmat(Mu(:,j), [1 n]);
            Znc = Z - repmat(Kappa(:,j), [1 n]);
            tmpProd = Lambda(:,:,j) * SigmaN(:,:,i) * Lambda(:,:,j)';
            for i=1:n
                tmpPsi(1:size(tmpPsi, 2) + 1:end) = tmpPsi(1:size(tmpPsi, 2) + 1:end) + ...
                    tmpQ(i) * (((Xnc(:,i) - Lambda(:,:,j) * Znc(:,i)) .^ 2)' + tmpProd(1:size(tmpProd, 2) + 1:end));
            end
            Psi(:,:,c) = tmpPsi;
        end

        % Update number of iterations
        iter = iter + 1;
        if rem(iter, 5) == 0
            fprintf('.');
        end
    end

    % Transpose to get lowdimensional data representation
    mappedX = Z';


%  Figure 10.51      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
%  fig10_51.m is a script to generate Fig. 10.51, the rootlocus
%  for the fuel/air ratio with a linear sensor.  A (3,3)
%  Pade approximant is used  to approximate the delay.
clf;
% construct the F/A dynamics with the sensor time-constant too
f =[-50 ,    0,     0;
     0 ,   -1 ,    0;
    10 ,   10 ,  -10];

g =[25.0000;
    0.5000;
         0];
h =[0,     0,     1];
j=0;
[n3,d3]=pade(0.2,3); %  the delay Pade model

[np,dp]=ss2tf(f,g,h,j); % convert to polynomial form

np=[np(3:4)];  % remove the extraneous leading zeros

np=conv(np,n3);  % the plant numerator
dp =conv(dp,d3);  % the plant denominator
nc=[1, .3];
dc=[1, 0]; % the PI controller in polynomial form
% form the open-loop system
nol=conv(np,nc);
dol=conv(dp,dc);

rlocus(nol,dol);
v=[-30 2 -12 12];  % set the axes
axis(v);
grid;
title('Fig. 10.51  Rootlocus for the F/A control')