function fb = acc_nav2body (acc_n, DCMnb_m)
% acc_nav2body: transforms accelerations from navigation frame to body
% frame.
%
% INPUT
%	acc_n: Nx3 matrix with [fn, fe, fd] accelerations in the navigation
%		frame.
%   DCMnb_m: Nx9 matrix with nav-to-body direct cosine matrices (DCM).
%       Each row of DCMnb_m contains the 9 elements of a particular DCMnb
%       matrix ordered as [a11 a21 a31 a12 a22 a32 a13 a23 a33].
%
% OUTPUT
%	fb: Nx3 matrix with [fx, fy, fz] simulated accelerations in the
%		body frame.
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% Reference:
%
%   R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
% simulation framework for low-cost integrated navigation systems,
% Journal of Control Engineering and Applied Informatics}, vol. 17,
% issue 2, pp. 110-120, 2015. Eq. 10.
%
% Version: 002
% Date:    2020/11/03
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

fb = zeros(size(acc_n));

M = max(size(acc_n));

for k = 1:M

    dcmnb = reshape(DCMnb_m(k,:), 3, 3);
    fb(k,:) = ( dcmnb * ( acc_n(k,:)' ) )';
end

end


function [L,Lc,Lli] = fromFramePlucker(C,Li)

% FROMFRAMEPLUCKER  Transform plucker line from a given frame.
%   L = FROMFRAMEPLUCKER(C,Li) expresses in global frame the line Li
%   originally expressed in frame C=(t,q).
%
%   The formula for transformation is
%
%       L = [ R        [t]_x*R ]
%           [ zeros(3)       R ] * Li
%
%   where
%       R    = q2R(q)
%      [t]_x = hat(t).
%
%   [L,Lc,Lli] = FROMFRAMEPLUCKER(...) returns the Jacobians wrt C
%   and Li.
%
%   See also TOFRAMEPLUCKER, TXP, HAT, CROSS, Q2R.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

ai = Li(1:3);
bi = Li(4:6);

t = C.t;
q = C.q;

if nargout == 1

    R = C.R;

    b = R*bi;
    a = R*ai + hat(t)*b;

    L = [a;b];

else

    [b,Bq,Bbi]      = Rp(q,bi);
    [txb,TXBt,TXBb] = crossJ(t,b);
    [Ra,RAq,RAai]   = Rp(q,ai);

    a   = Ra + txb;

    At  = TXBt;
    Aq  = RAq + TXBb*Bq;
    Aai = RAai;
    Abi = TXBb*Bbi;

    L   = [a;b];

    Lc  = [At Aq;zeros(3) Bq];
    Lli = [Aai Abi;zeros(3) Bbi];

end

return

%% test jacobians

syms a b c d x y z real
syms L1 L2 L3 L4 L5 L6 real

q   = [a;b;c;d];
t   = [x;y;z];
C.x = [t;q];
C   = updateFrame(C);
Li  = [L1;L2;L3;L4;L5;L6];

[L,Lc,Lli] = fromFramePlucker(C,Li);

simplify(Lc  - jacobian(L,C.x))
simplify(Lli - jacobian(L,Li))

%% test inv. transform
Li2 = toFramePlucker(C,L);

EL = simplify(Li - Li2);

simplify(subs(EL,d,sqrt(1-a^2-b^2-c^2)))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


%% edgeListToCurve
% Below is a demonstration of the features of the |edgeListToCurve| function

%%
clear; close all; clc;

%% Syntax
% |[indList]=edgeListToCurve(E);|

%% Description
% This function converts the nx2 edges array |E|, which should define a
% single curve, to an ordered list of mx1 indices |indList| for that curve.
% Note that iff the edges define a closed curve the first and last indices
% in the ordered list are the same and therefore repeated.

%% Examples
%

%%
% Plot settings
markerSize=40;
lineWidth=4;
fontSize=25;

%% Getting indices for a single boundary curve

%%
% Creating example geometry
t=linspace(0,2*pi,50); t=t(1:end-1); %Angles
V1=[cos(t(:)) sin(t(:))];

%Desired point spacing
pointSpacing=0.5;

[F,V]=regionTriMesh2D({V1},pointSpacing,1,0);

%%
% Get boundary edges
Eb=patchBoundary(F);

%%
% Use |edgeListToCurve| to get curve indices
indList=edgeListToCurve(Eb);

%%
% Visualize example mesh and boundary edges/curves

cFigure;
subplot(1,2,1); hold on;
title('Boundary edges','FontSize',fontSize)
hp1=gpatch(F,V,'kw','k',1,1);
hp2=gpatch(Eb,V,'none',(1:1:size(Eb,1))',1,lineWidth);
legend([hp1 hp2],{'Mesh','Boundary edges'});
axisGeom(gca,fontSize); view(2);

subplot(1,2,2); hold on;
title('Boundary curve','FontSize',fontSize)
hp1=gpatch(F,V,'kw','k',1,1);
hp2=plotV(V(indList,:),'r.-','MarkerSize',markerSize,'LineWidth',lineWidth);
legend([hp1 hp2],{'Mesh','Ordered boundary curve'});
axisGeom(gca,fontSize); view(2);

drawnow;

%% Getting indices for a multiple boundary curves

%%
% Creating example geometry with multiple boundary sets

%Boundary 1
ns=150;
t=linspace(0,2*pi,ns);
t=t(1:end-1);
r=6+2.*sin(5*t);
[x,y] = pol2cart(t,r);
V1=[x(:) y(:)];

%Boundary 2
[x,y] = pol2cart(t,ones(size(t)));
V2=[x(:) y(:)+4];

%Boundary 3
[x,y] = pol2cart(t,2*ones(size(t)));
V3=[x(:) y(:)-0.5];

%Defining a region
regionCell={V1,V2,V3}; %A region between V1 and V2 (V2 forms a hole inside V1)

plotOn=1; %This turns on/off plotting

%Desired point spacing
pointSpacing=0.5;

[F,V]=regionTriMesh2D(regionCell,pointSpacing,1,0);

%%
% Get boundary edges
Eb=patchBoundary(F);

%%
% Use grouping to "seperate" boundary sets
optionStruct.outputType='label';
G=tesgroup(Eb,optionStruct);

%%

cFigure; hold on;
gpatch(F,V,'kw','k',1,1);
axisGeom(gca,fontSize); view(2);

plotColors=gjet(max(G(:)));
for q=1:1:max(G(:))

    E_now=Eb(G==q,:);

    plotV(V(E_now,:),'b.','markersize',25);

    [indListNow]=edgeListToCurve(E_now);

    hp=plotV(V(indListNow,:),'b.-','MarkerSize',markerSize,'LineWidth',lineWidth);
    hp.Color=plotColors(q,:);

end

drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*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/>.


function dx = pend_dx(u, x)

M = 1;   % kg
m = 0.3; % kg
g = 9.81;% m s^-2
L = 0.6; % m

a = x(2);
da = x(4);

if a == 0
	sinca = 1;
else
	sinca = sin(a)/a;
end

E = [...
	eye(2,2) zeros(2,2);
	zeros(2,2) [M/m+1, L*cos(a); cos(a), 4/3*L]];
A = [...
	zeros(2,2) eye(2,2);
	[0 0; 0 g*sinca] [0 L*da*sin(a); 0 0]];

B = [0 0 1/m 0]';

% E dx = A x + B u
piE = pinv(E);
dx = piE*A*x + piE*B*u;


% This function belongs to Piotr Dollar's Toolbox
% http://vision.ucsd.edu/~pdollar/toolbox/doc/index.html
% Please refer to the above web page for definitions and clarifications
%
% Calculates the distance between sets of vectors.
%
% Let X be an m-by-p matrix representing m points in p-dimensional space
% and Y be an n-by-p matrix representing another set of points in the same
% space. This function computes the m-by-n distance matrix D where D(i,j)
% is the distance between X(i,:) and Y(j,:).  This function has been
% optimized where possible, with most of the distance computations
% requiring few or no loops.
%
% The metric can be one of the following:
%
% 'euclidean' / 'sqeuclidean':
%   Euclidean / SQUARED Euclidean distance.  Note that 'sqeuclidean'
%   is significantly faster.
%
% 'chisq'
%   The chi-squared distance between two vectors is defined as:
%    d(x,y) = sum( (xi-yi)^2 / (xi+yi) ) / 2;
%   The chi-squared distance is useful when comparing histograms.
%
% 'cosine'
%   Distance is defined as the cosine of the angle between two vectors.
%
% 'emd'
%   Earth Mover's Distance (EMD) between positive vectors (histograms).
%   Note for 1D, with all histograms having equal weight, there is a simple
%   closed form for the calculation of the EMD.  The EMD between histograms
%   x and y is given by the sum(abs(cdf(x)-cdf(y))), where cdf is the
%   cumulative distribution function (computed simply by cumsum).
%
% 'L1'
%   The L1 distance between two vectors is defined as:  sum(abs(x-y));
%
%
% USAGE
%  D = pdist2( X, Y, [metric] )
%
% INPUTS
%  X        - [m x p] matrix of m p-dimensional vectors
%  Y        - [n x p] matrix of n p-dimensional vectors
%  metric   - ['sqeuclidean'], 'chisq', 'cosine', 'emd', 'euclidean', 'L1'
%
% OUTPUTS
%  D        - [m x n] distance matrix
%
% EXAMPLE
%  [X,IDX] = demoGenData(100,0,5,4,10,2,0);
%  D = pdist2( X, X, 'sqeuclidean' );
%  distMatrixShow( D, IDX );
%
% See also PDIST, DISTMATRIXSHOW

% Piotr's Image&Video Toolbox      Version 2.0
% Copyright (C) 2007 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Lesser GPL [see external/lgpl.txt]

function D = pdist2(X, Y, metric)

if( nargin<3 || isempty(metric) ); metric=0; end;

switch metric
  case 'sqeuclidean'
    D = distEucSq( X, Y );
  case {0, 'euclidean'}
    D = sqrt(distEucSq( X, Y ));
  case 'L1'
    D = distL1( X, Y );
  case 'cosine'
    D = distCosine( X, Y );
  case 'emd'
    D = distEmd( X, Y );
  case 'chisq'
    D = distChiSq( X, Y );
  otherwise
    error(['pdist2 - unknown metric: ' metric]);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function D = distL1( X, Y )

m = size(X,1);  n = size(Y,1);
mOnes = ones(1,m); D = zeros(m,n);
for i=1:n
  yi = Y(i,:);  yi = yi( mOnes, : );
  D(:,i) = sum( abs( X-yi),2 );
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function D = distCosine( X, Y )

if( ~isa(X,'double') || ~isa(Y,'double'))
  error( 'Inputs must be of type double'); end;

p=size(X,2);
XX = sqrt(sum(X.*X,2)); X = X ./ XX(:,ones(1,p));
YY = sqrt(sum(Y.*Y,2)); Y = Y ./ YY(:,ones(1,p));
D = 1 - X*Y';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function D = distEmd( X, Y )

Xcdf = cumsum(X,2);
Ycdf = cumsum(Y,2);

m = size(X,1);  n = size(Y,1);
mOnes = ones(1,m); D = zeros(m,n);
for i=1:n
  ycdf = Ycdf(i,:);
  ycdfRep = ycdf( mOnes, : );
  D(:,i) = sum(abs(Xcdf - ycdfRep),2);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function D = distChiSq( X, Y )

%%% supposedly it's possible to implement this without a loop!
m = size(X,1);  n = size(Y,1);
mOnes = ones(1,m); D = zeros(m,n);
for i=1:n
  yi = Y(i,:);  yiRep = yi( mOnes, : );
  s = yiRep + X;    d = yiRep - X;
  D(:,i) = sum( d.^2 ./ (s+eps), 2 );
end
D = D/2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function D = distEucSq( X, Y )

%if( ~isa(X,'double') || ~isa(Y,'double'))
 % error( 'Inputs must be of type double'); end;
m = size(X,1); n = size(Y,1);
%Yt = Y';
XX = sum(X.*X,2);
YY = sum(Y'.*Y',1);
D = XX(:,ones(1,n)) + YY(ones(1,m),:) - 2*X*Y';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function D = distEucSq( X, Y )
%%%% code from Charles Elkan with variables renamed
% m = size(X,1); n = size(Y,1);
% D = sum(X.^2, 2) * ones(1,n) + ones(m,1) * sum(Y.^2, 2)' - 2.*X*Y';

%%% LOOP METHOD - SLOW
% [m p] = size(X);
% [n p] = size(Y);
%
% D = zeros(m,n);
% onesM = ones(m,1);
% for i=1:n
%   y = Y(i,:);
%   d = X - y(onesM,:);
%   D(:,i) = sum( d.*d, 2 );
% end

%%% PARALLEL METHOD THAT IS SUPER SLOW (slower then loop)!
% % From "MATLAB array manipulation tips and tricks" by Peter J. Acklam
% Xb = permute(X, [1 3 2]);
% Yb = permute(Y, [3 1 2]);
% D = sum( (Xb(:,ones(1,n),:) - Yb(ones(1,m),:,:)).^2, 3);

%%% USELESS FOR EVEN VERY LARGE ARRAYS X=16000x1000!! and Y=100x1000
% call recursively to save memory
% if( (m+n)*p > 10^5 && (m>1 || n>1))
%   if( m>n )
%     X1 = X(1:floor(end/2),:);
%     X2 = X((floor(end/2)+1):end,:);
%     D1 = distEucSq( X1, Y );
%     D2 = distEucSq( X2, Y );
%     D = cat( 1, D1, D2 );
%   else
%     Y1 = Y(1:floor(end/2),:);
%     Y2 = Y((floor(end/2)+1):end,:);
%     D1 = distEucSq( X, Y1 );
%     D2 = distEucSq( X, Y2 );
%     D = cat( 2, D1, D2 );
%   end
%   return;
% end


function [TRI4, x4, y4, z4]=make_sphere(TRI, x, y, z)
% % Syntax;
% %
% % [TRI3, x3, y3, z3]=make_sphere(TRI, x, y, z);
% %
% % ***********************************************************
% %
% % Description
% %
% % Program makes the dome of an even frequency icosahedron
% % into a full spherical icosahedron.
% %
% % ***********************************************************
% %
% % Input Variables
% %
% % x, y,and z are row vector of the x, y, and z coordinates of the
% % icosahedron nodes of the dome.
% %
% % TRI is the triangularization of the icosahedron nodes.
% %
% % ***********************************************************
% %
% % Output Variables
% %
% % x4,y4,and z4 are row vector of the x4, y4, and z4 coordinates of the
% % full spherical icosahedron nodes.
% %
% % TRI4 is the triangularization of the spherical icosahedron nodes.
% %
% % ***********************************************************
% %
% Example
%
% f=2; % 2 freuqency similar to soccer ball
%
% r=4; % 4 meter radius
%
% [x2, y2, z2, it]=icosahedron_nodes(f, r);
%
% [x, y, z]=build_icosa(x2, y2, z2);
%
% [rho, theta, phi]=spherical_angle_ed(x, y, z);
%
% [x10, y10, z10]=splat(rho, theta, phi);
%
% TRI = delaunay(x10, y10);
%
% [TRI, x, y, z]=make_sphere(TRI, x, y, z);
%
% figure(1);
% h=trisurf(TRI,x,y,z, 'FaceColor', [1 0 0], 'EdgeColor', 0*[1 1 1], ...
% 'LineWidth', 1 );
% title('Red Soccer Ball');
% axis equal
%
% % ***********************************************************
% %
% % This program was written by Edward L. Zechmann
% %
% % date 7   March   2008
% %
% % modified 11   March   2008  added examples and comments
% %
% % ***********************************************************
% %
% % Feel free to modify this code.
% %

nn=length(x);
r=sqrt((x(1, 1).^2+y(1, 1).^2+z(1, 1).^2));

% Calculate the frequency from the length of x
% Assuming that x,y,and z are for a dome.
% length(x)=5*f^2+5/2*f+1;
% using the quadratic equation
f=round(0.25*(sqrt(1/5*(16*length(x)-11))-1));

% For a full sphere the freuqency can be caculated from teh number
% of nodes by the formulas
% length(x)=10*f^2+2;
% Using the quadratic equation
% f=round(sqrt((length(x)-2)/10));

ix1=find( abs(z) < 0.01*r/(f+1));
ix2=setdiff(1:nn, ix1);

trix2=[];
trix3=[];
m=length(TRI);

for e1=1:m;

   tri1=TRI(e1, :);

    aa=intersect(tri1,ix1);

    if ~isempty(aa)
        trix2=[trix2 e1];
    else
        trix3=[trix3 e1];
    end

end

TRI2=TRI(trix2, :);
TRI3=TRI(trix3, :);
TRI33=zeros(size(TRI3));

for e1=1:length(TRI3);
    for e2=1:3;
        TRI33(e1, e2)=find(TRI3(e1, e2)==ix2);
    end
end

TRI22=zeros(size(TRI2));

for e1=1:length(TRI2);
    for e2=1:3;

        ix3=find(TRI2(e1, e2)==ix1);
        if ~isempty(ix3)
            TRI22(e1, e2)=ix1(ix3);
        else
            ix4=nn+find(TRI2(e1, e2)==ix2);
            TRI22(e1, e2)=ix4;
        end

    end
end

x4=[x x(ix2)];
y4=[y y(ix2)];
z4=[z -z(ix2)];

TRI4=[TRI' [nn+TRI33]' TRI22']';


function example
% Two-state model of gene expression
%
% Reaction network:
%   0 -> mRNA
%   mRNA -> mRNA + protein
%   mRNA -> 0
%   protein -> 0

tspan = [0, 10000]; %seconds
x0 = [0, 0]; %mRNA, protein
stoich_matrix = [ 1  0  ; %transcription
                  0  1  ; %translation
                 -1  0  ; %mRNA degradation
                  0 -1 ]; %protein degradation

% Rate constants
p.kR = 0.1;%0.01;
p.kP = 0.1;%1;
p.gR = 0.1;
p.gP = 0.002;

% Run simulation
%[t,x] = directMethod(stoich_matrix, @propensities_2state, tspan, x0, p);
[t,x] = firstReactionMethod(stoich_matrix, @propensities_2state, tspan, x0, p);

% Plot time course
figure(gcf);
stairs(t,x);
set(gca,'XLim',tspan);
xlabel('time (s)');
ylabel('molecules');
legend({'mRNA','protein'});

end

function a = propensities_2state(x, p)
mRNA    = x(1);
protein = x(2);

a = [p.kR;
     p.kP*mRNA;
	 p.gR*mRNA;
     p.gP*protein];
end


function quadrule_test065 ( n )

%*****************************************************************************80
%
%% QUADRULE_TEST065 uses CHEBSHEV2_COMPUTE to integral over the semicircle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 January 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUADRULE_TEST065\n' );
  fprintf ( 1, '  Approximate the integral of f(x,y) over the semicircle\n' );
  fprintf ( 1, '    -1 <= x <= 1, y = sqrt ( 1 - x^2 )\n' );
  fprintf ( 1, '  using N Chebyshev points.\n' );
  fprintf ( 1, '  If p(x,y) involves any term of odd degree in y,\n' );
  fprintf ( 1, '  the estimate will only be approximate.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Polynomial    N    Integral        Estimate       Error\n' );
  fprintf ( 1, '\n' );

  [ x, w ] = chebyshev2_compute ( n );
%
%  f(x,y) = 1
%
  exact = 1.5707963267948966192;
  f(1:n,1) = 1.0;
  q = w(1:n)' * f(1:n);
  error = abs ( q - exact );
  fprintf ( 1, '  1            %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = x
%
  exact = 0.0;
  f(1:n,1) = x(1:n);
  q = w(1:n)' * f(1:n);
  error = abs ( q - exact );
  fprintf ( 1, '  x            %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = y = sqrt ( 1 - x^2 )
%
  exact = 0.66666666666666666667;
  f(1:n,1) = sqrt ( 1.0 - x(1:n).^2 );
  q = w(1:n)' * f(1:n) / 2.0;
  error = abs ( q - exact );
  fprintf ( 1, '     y         %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = x^2
%
  exact = 0.39269908169872415481;
  f(1:n,1) = x(1:n).^2;
  q = w(1:n)' * f(1:n);
  error = abs ( q - exact );
  fprintf ( 1, '  x^2          %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = xy = x * sqrt ( 1 - x^2 )
%
  exact = 0.0;
  f(1:n,1) = x(1:n) .* sqrt ( 1.0 - x(1:n).^2 );
  q = w(1:n)' * f(1:n) / 2.0;
  error = abs ( q - exact );
  fprintf ( 1, '  x  y         %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = y^2 -> ( 1 - x^2 )
%
  exact = 0.39269908169872415481;
  f(1:n,1) = 1.0 - x(1:n).^2;
  q = w(1:n)' * f(1:n) / 3.0;
  error = abs ( q - exact );
  fprintf ( 1, '     y^2       %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = x^3
%
  exact = 0.0;
  f(1:n,1) = x(1:n).^3;
  q = w(1:n)' * f(1:n);
  error = abs ( q - exact );
  fprintf ( 1, '  x^3          %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = x^2 y = x^2 sqrt ( 1 - x^2 )
%
  exact = 0.13333333333333333333;
  f(1:n,1) = x(1:n).^2 .* sqrt ( 1.0 - x(1:n).^2 );
  q = w(1:n)' * f(1:n) / 2.0;
  error = abs ( q - exact );
  fprintf ( 1, '  x^2y         %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = x y^2 = x * ( 1 - x^2 )
%
  exact = 0.0;
  f(1:n,1) = x(1:n) .* ( 1.0 - x(1:n).^2 );
  q = w(1:n)' * f(1:n) / 3.0;
  error = abs ( q - exact );
  fprintf ( 1, '  x  y^2       %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = y^3
%
  exact = 0.26666666666666666667;
  f(1:n,1) = ( 1.0 - x(1:n).^2 ).^(1.5);
  q = w(1:n)' * f(1:n) / 4.0;
  error = abs ( q - exact );
  fprintf ( 1, '     y^3       %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = x^4
%
  exact = 0.19634954084936207740;
  f(1:n,1) = x(1:n).^4;
  q = w(1:n)' * f(1:n);
  error = abs ( q - exact );
  fprintf ( 1, '  x^4          %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = x^2y^2 -> x^2( 1 - x^2 )
%
  exact = 0.065449846949787359135;
  f(1:n,1) = x(1:n).^2 .* ( 1.0 - x(1:n).^2 );
  q = w(1:n)' * f(1:n) / 3.0;
  error = abs ( q - exact );
  fprintf ( 1, '  x^2y^2       %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = y^4 -> ( 1 - x^2 )^2
%
  exact = 0.19634954084936207740;
  f(1:n,1) = ( 1.0 - x(1:n).^2 ).^2;
  q = w(1:n)' * f(1:n) / 5.0;
  error = abs ( q - exact );
  fprintf ( 1, '     y^4       %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = x^4y = x^4 sqrt ( 1 - x^2 )
%
  exact = 0.057142857142857142857;
  f(1:n,1) = x(1:n).^4 .* sqrt ( 1.0 - x(1:n).^2 );
  q = w(1:n)' * f(1:n) / 2.0;
  error = abs ( q - exact );
  fprintf ( 1, '  x^4y         %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  x^2y^3 = x^2 ( 1 - x^2 )**(3/2)
%
  exact = 0.038095238095238095238;
  f(1:n,1) = x(1:n).^2 .* ( 1.0 - x(1:n).^2 ).^(1.5);
  q = w(1:n)' * f(1:n) / 4.0;
  error = abs ( q - exact );
  fprintf ( 1, '  x^2y^3       %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = y^5
%
  exact = 0.15238095238095238095;
  f(1:n,1) = ( 1.0 - x(1:n).^2 ).^(2.5);
  q = w(1:n)' * f(1:n) / 6.0;
  error = abs ( q - exact );
  fprintf ( 1, '     y^5       %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = x^6
%
  exact = 0.12271846303085129838;
  f(1:n,1) = x(1:n).^6;
  q = w(1:n)' * f(1:n);
  error = abs ( q - exact );
  fprintf ( 1, '  x^6          %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = x^4y^2 -> x^4( 1 - x^2 )
%
  exact = 0.024543692606170259675;
  f(1:n,1) = x(1:n).^4 .* ( 1.0 - x(1:n).^2 );
  q = w(1:n)' * f(1:n) / 3.0;
  error = abs ( q - exact );
  fprintf ( 1, '  x^4y^2       %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = x^2y^4 -> x^2( 1 - x^2 )^2
%
  exact = 0.024543692606170259675;
  f(1:n,1) = x(1:n).^2 .* ( 1.0 - x(1:n).^2 ).^2;
  q = w(1:n)' * f(1:n) / 5.0;
  error = abs ( q - exact );
  fprintf ( 1, '  x^2y^4       %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );
%
%  f(x,y) = y^6 -> ( 1 - x^2 )^3
%
  exact = 0.12271846303085129838;
  f(1:n,1) = ( 1.0 - x(1:n).^2 ).^3;
  q = w(1:n)' * f(1:n) / 7.0;
  error = abs ( q - exact );
  fprintf ( 1, '     y^6       %2d  %14.6g  %14.6g  %14.6g\n', n, exact, q, error );

  return
end


function out = proj_halfspace_box(x,a,b,lb,ub)
%PROJ_HALFSPACE_BOX computes the orthogonal projection onto the intersection of a halfspace and a box
%                   {x : <a,x> <= b, lb<=x<=ub}
%
%  Usage:
%  out = PROJ_HALFSPACE_BOX(x,a,b,[lb],[ub])
%  ===========================================
%  Input:
%  x - point to be projected (vector/matrix)
%  a - vector/matrix
%  b - scalar
%  lb - lower bound (vector/matrix/scalar) [default: -inf]
%  ub - upper bound (vector/matrix/scalar) [default: inf]
%  ===========================================
%  Assumptions:
%  The intersection of the halfspace and the box is nonempty
%  ===========================================
%  Output:
%  out - projection vector

% This file is part of the FOM package - a collection of first order methods for solving convex optimization problems
% Copyright (C) 2017 Amir and Nili Beck
%
% 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/>.

%reading the user x and setting defalut values when required.
if (nargin < 3)
    error ('usage: proj_halfspace_box(x,a,b,[lb],[ub]') ;
end

if (nargin < 5)
    %upper bound is not given, setting to defalut value :inf
    ub = inf ;
end
if ((nargin < 4) || (isempty( lb)))
    %lower bound is not given, setting to defalut value :0
    lb = -inf;
end

if  (any(any(lb > ub)))
    error('Set is infeasible') ;
end

if trace(a'*min(max(x,lb),ub)) <= b
    out =  min(max(x,lb),ub) ;
else
    %use proj_hyperplane_box
    out = proj_hyperplane_box(x,a,b,lb,ub) ;
end


function value = hartley_determinant ( n )

%*****************************************************************************80
%
%% HARTLEY_DETERMINANT returns the determinant of the HARTLEY matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real VALUE, the determinant.
%
  if ( mod ( n, 4 ) == 1 )
    value =   sqrt ( n ^ n );
  else
    value = - sqrt ( n ^ n );
  end

  return
end


function [a] = tcoeffs(X,P,window,weight,nModes)
%TCOEFFS Continuously-discrete temporal expansion coefficients of SPOD
%modes
%   [A] = TCOEFFS(X,P,WINDOW,WEIGHT,NMODES) returns the
%   continuously-discrete temporal SPOD mode expansion coefficients of the
%   leading NMODES modes. P is the data matrix of SPOD modes returned by
%   SPOD. X, WINDOW and WEIGHT are the same variables as for SPOD. If no
%   windowing function is specified, a Hamming window of length WINDOW will
%   be used. If WEIGHT is empty, a uniform weighting of 1 is used.
%
%   Reference:
%     [1] A. Nekkanti, O. T. Schmidt, Frequency–time analysis, low-rank
%         reconstruction and denoising of turbulent flows using SPOD,
%         Journal of Fluid Mechanics 926, A26, 2021
%
% A. Nekkanti (aknekkan@eng.ucsd.edu), O. T. Schmidt (oschmidt@ucsd.edu)
% Last revision: 7-Oct-2022 Brandon Yeung <byeung@ucsd.edu>

dims        = size(X);
nt          = dims(1);
nGrid       = prod(dims(2:end));

window = window(:); weight = weight(:);

% default window size and type
if length(window)==1
    window  = hammwin(window);
end

nDFT        = length(window);
winWeight   = 1/mean(window);

% inner product weight
if isempty(weight)
    weight  = ones(nGrid,1);
end

X           = reshape(X,nt,nGrid);
X           = X-mean(X,1);              % subtract mean

ndims       = size(P);
P           = permute(P,[1 length(ndims) 2:length(ndims)-1]);
if isreal(X)
    nFreq   = ceil(nDFT/2)+1;
    P       = reshape(P,ceil(nDFT/2)+1,ndims(end),nGrid);
else
    nFreq   = nDFT;
    P       = reshape(P,nDFT,ndims(end),nGrid);
end

weight     = reshape(weight,1,nGrid);
% zero-padding
X          = [zeros(ceil(nDFT/2),nGrid); X; zeros(ceil(nDFT/2),nGrid);];
a          = zeros(nFreq,nModes,nt);
winCorr_fac= winWeight/nDFT;
disp(' ')
disp('Calculating expansion coefficients')
disp('------------------------------------')
for i=1:nt
    X_blk               = fft(X(i:i+nDFT-1,:).*window);
    X_blk               = X_blk(1:nFreq,:);
    % correction for windowing and zero-padding
    if (i<ceil(nDFT/2)+1)
        corr 	= 1/(winCorr_fac*sum(window(ceil(nDFT/2)-i+1:nDFT)));
    elseif (i>nt-ceil(nDFT/2)+1)
        corr	= 1/(winCorr_fac*sum(window(1:nt+ceil(nDFT/2)-i)));
    else
        corr 	= 1;
    end
    for l=1:nModes
        a(:,l,i) = corr*winCorr_fac*dot(squeeze(P(:,l,:)),weight.*X_blk,2);
    end
    disp(['time ' num2str(i) '/' num2str(nt)])
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [window] = hammwin(N)
%HAMMWIN Standard Hamming window of lenght N
    window = 0.54-0.46*cos(2*pi*(0:N-1)/(N-1))';
end


function point_num = levels_index_size_onn ( dim_num, level_max )

%*****************************************************************************80
%
%% LEVELS_INDEX_SIZE_ONN sizes a sparse grid made from ONN 1D rules.
%
%  Discussion:
%
%    The sparse grid is presumed to have been created from products
%    of OPEN NON-NESTED 1D quadrature rules.
%
%    ONN rules include Gauss Laguerre.
%
%    The sparse grid is the logical sum of product grids with total LEVEL
%    between LEVEL_MIN and LEVEL_MAX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2008
%
%  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.
%
%    Output, integer POINT_NUM, the number of points in the grid.
%

%
%  Special case.
%
  if ( level_max == 0 )
    point_num = 1;
    return
  end
%
%  The outer loop generates LEVELs from LEVEL_MIN to LEVEL_MAX.
%
  level_min = max ( 0, level_max + 1 - dim_num );

  point_num = 0;

  for level = level_min : level_max
%
%  The middle loop generates the next partition that adds up to LEVEL.
%
    level_1d = [];
    more = 0;
    h = 0;
    t = 0;

    while ( 1 )

      [ level_1d, more, h, t ] = comp_next ( level, dim_num, level_1d, more, h, t );
%
%  Transform each 1D level to a corresponding 1D order.
%
      order_1d = level_to_order_open ( dim_num, level_1d );

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

      if ( ~more )
        break
      end

    end
  end

  return
end


function SR

clc;
clear;
close all;
%% the example fuction
datasize=200;
var_noise=0.1;
x=linspace(-10,10,datasize)';
    f=sin(x)./x;
noise=var_noise*randn(datasize,1);
y=f+noise;
x_raw=x;
plot(x_raw,f,'r-','LineWidth',2),hold on;
%%
Inset=30;
dataindex=randsample(datasize,Inset);
xI=x(dataindex);
yI=y(dataindex);
%%
%%test
testnum=500;
x_test=linspace(-15,15,testnum)';

K_mn=Kernel_func(xI,x,0.7,1.2);
K_mm=Kernel_func(xI,xI,0.7,1.2);
 rank(K_mm)
K_mat=K_mn*1/var_noise*eye(datasize)*K_mn'+K_mm;
K_sm=Kernel_func(x_test,xI,0.7,1.2);
L=chol(K_mat,'lower');
b=K_mn*1/var_noise*eye(datasize)*y;
f_mean=K_sm*(L'\(L\b));

for i=1:testnum
K_sm=Kernel_func(x_test(i),xI,0.7,1.2);
v=L\(K_sm');
  f_var(i)=v'*v;
end
f_var=f_var';

plot(x_test,f_mean,'b-','LineWidth',2),hold on;

x_area=[x_test;flipdim(x_test,1)];
ybound_std=[(f_mean+1.96*sqrt(f_var));flipdim(f_mean-1.96*sqrt(f_var),1)];
fill(x_area,ybound_std,[187 255 0]/255,'EdgeColor','none');
plot(x_raw,f,'r-','LineWidth',2),hold on;
plot(xI,yI,'b+'),hold on;
plot(x_test,f_mean,'b-','LineWidth',2),hold on;
%%
legend1=legend('target function','training points','preditive distribution','95% confidence interval');
 set(legend1,'Box','off','Color','none',...
     'Location','NorthEast');
xlabel('(f)SR with 30 points ')
xlim([-15,15]);
ylim([-2,2]);
set(gca,'XTick',[-15:3:15])
matlab2tikz( 'SubReg30.tex' )


function value = dasum ( n, x, incx )

%*****************************************************************************80
%
%% DASUM takes the sum of the absolute values of a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, real X(*), the vector to be examined.
%
%    Input, integer INCX, the increment between successive entries of X.
%    INCX must not be negative.
%
%    Output, real VALUE, the sum of the absolute values of X.
%
  value = sum ( abs ( x(1:incx:1+(n-1)*incx) ) );

  return
end


classdef DASCMOP8 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Difficulty-adjustable and scalable constrained benchmark MOP

%------------------------------- Reference --------------------------------
% Z. Fan, W. Li, X. Cai, H. Li, C. Wei, Q. Zhang, K. Deb, and E. Goodman,
% Difficulty adjustable and scalable constrained multi-objective test
% problem toolkit, Evolutionary Computation, 2020, 28(3): 339-378.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Wenji Li

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 3;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            x_all = X(:,3:1:end);
            g_1   = sum((x_all - 0.5) .* (x_all - 0.5) - cos(20.0 * pi .* ( x_all - 0.5)),2);
            sum1  = obj.D - 2 + g_1;
            PopObj(:,1) = cos(0.5 * pi * X(:,1)) .* cos(0.5 * pi * X(:,2)) + sum1;
            PopObj(:,2) = cos(0.5 * pi * X(:,1)) .* sin(0.5 * pi * X(:,2)) + sum1;
            PopObj(:,3) = sin(0.5 * pi * X(:,1)) + sum1;
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,X)
            x_all  = X(:,3:1:end);
            g_1    = sum((x_all - 0.5) .* (x_all - 0.5) - cos(20.0 * pi .* ( x_all - 0.5)),2);
            sum1   = obj.D - 2 + g_1;
            PopCon = Constraint(X(:,1),X(:,2),sum1);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,3);
            R = R./repmat(sqrt(sum(R.^2,2)),1,3);
            X(:,2) = atan(R(:,2)./R(:,1))/0.5/pi;
            X(:,1) = acos(R(:,1)./cos(0.5*pi*X(:,2)))/0.5/pi;
            C(:,1) = -sin(20*pi*X(:,1));
            C(:,2) = -cos(20*pi*X(:,2));
            R(any(C>1e-2,2),:) = [];
            R = R + 0.5;
        end
        %% Generate the feasible region
        function R = GetPF(obj)
            [X1,X2] = meshgrid(linspace(0,1,100));
            x = cos(0.5*pi*X1).*cos(0.5*pi*X2);
            y = cos(0.5*pi*X1).*sin(0.5*pi*X2);
            z = sin(0.5*pi*X1);
            fes = all(Constraint(X1(:),X2(:),0.5)<=0,2);
            z(reshape(~fes,size(z))) = nan;
            R = {x+0.5,y+0.5,z+0.5};
        end
    end
end

function PopCon = Constraint(X1,X2,sum1)
    % set the parameters of constraints
    DifficultyFactors = [0.5,0.5,0.5];
    % Type-I parameters
    a = 20;
    b = 2 * DifficultyFactors(1) - 1;
    % Type-II parameters
    d = 0.5;
    if DifficultyFactors(2) == 0.0
        d = 0.0;
    end
    e = d - log(DifficultyFactors(2));
    if isfinite(e) == 0
        e = 1e+30;
    end
    % Type-III parameters
    r = 0.5 * DifficultyFactors(3);
    % Calculate objective values
    PopObj(:,1) = cos(0.5 * pi * X1) .* cos(0.5 * pi * X2) + sum1;
    PopObj(:,2) = cos(0.5 * pi * X1) .* sin(0.5 * pi * X2) + sum1;
    PopObj(:,3) = sin(0.5 * pi * X1) + sum1;
    % Type-I constraints
    PopCon(:,1) = b - sin(a * pi * X1);
    PopCon(:,2) = b - cos(a * pi * X2);
    % Type-II constraints
    PopCon(:,3) = -(e - sum1) .* (sum1 - d);
    if DifficultyFactors(2) == 1.0
        PopCon(:,3) = 1e-4 - abs(sum1 - e);
    end
    % Type-III constraints
    x_k = [1.0, 0.0, 0.0, 1.0 / sqrt(3.0)];
    y_k = [0.0, 1.0, 0.0, 1.0 / sqrt(3.0)];
    z_k = [0.0, 0.0, 1.0, 1.0 / sqrt(3.0)];
    for k=1:length(x_k)
        PopCon(:,3+k) = r * r - ((PopObj(:,1) - x_k(k))).^2  -...
            ((PopObj(:,2) - y_k(k))).^2 - ((PopObj(:,3) - z_k(k))).^2;
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  MATLAB Code for                                                  %
%                                                                   %
%  Multi-Objective Particle Swarm Optimization (MOPSO)              %
%  Version 1.0 - Feb. 2011                                          %
%                                                                   %
%  According to:                                                    %
%  Carlos A. Coello Coello et al.,                                  %
%  "Handling Multiple Objectives with Particle Swarm Optimization," %
%  IEEE Transactions on Evolutionary Computation, Vol. 8, No. 3,    %
%  pp. 256-279, June 2004.                                          %
%                                                                   %
%  Developed Using MATLAB R2009b (Version 7.9)                      %
%                                                                   %
%  Programmed By: S. Mostapha Kalami Heris                          %
%                                                                   %
%         e-Mail: sm.kalami@gmail.com                               %
%                 kalami@ee.kntu.ac.ir                              %
%                                                                   %
%       Homepage: http://www.kalami.ir                              %
%                                                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function z=MyCost2(x)

    z1=sum(-10*exp(-0.2*sqrt(x(1:end-1).^2+x(2:end).^2)));

    z2=sum(abs(x).^0.8+5*(sin(x.^3)));

    z=[z1 z2];

end


function [xi] = log_multiSE3(chi)
C = chi(1:3,1:3);
r = chi(1:3,4:end);

[vecs,eigs] = eig(C);
[~,idx] = min(abs(diag(eigs)-1));
a = vecs(:,idx);

phi = acos(1/2*(trace(C)-1));
if phi == 0
    a = zeros(3,1);
    iJ = eye(3);
else
    A = [0 -a(3) a(2);a(3) 0 -a(1);-a(2) a(1) 0];
    a_ta = a*transpose(a);
    C1 = cos(phi)*eye(3) + (1-cos(phi))*a_ta + sin(phi)*A;
    C2 = cos(-phi)*eye(3) + (1-cos(-phi))*a_ta + sin(-phi)*A;
    if norm(C2-C)<norm(C1-C)
        phi = -phi;
    end
    iJ = phi/2*cot(phi/2)*eye(3) + (1-phi/2*cot(phi/2))*a_ta - phi/2*A;
end
rho = iJ*r;
xi = real([phi*a;rho(:)]);
end


function Q = spm_dcm_csd_Q(csd)
% Precision of cross spectral density
% FORMAT Q = spm_dcm_csd_Q(csd)
%
% csd{i}   - [cell] Array of complex cross spectra
% Q        - normalised precision
%--------------------------------------------------------------------------
% This routine returns the precision of complex cross spectra based upon
% the asymptotic results described in Camba-Mendez & Kapetanios (2005):
% In particular, the scaled difference between the sample spectral density
% (g) and the predicted density (G);
%
% e = vec(g - G)
%
% is asymptotically complex normal, where the covariance between e(i,j) and
% e(u,v) is given by Q/h and:
%
% Q = G(i,u)*G(j,u):  h = 2*m + 1
%
% Here m represent the number of averages from a very long time series.The
% inverse of the covariance is thus a scaled precision, where the
% hyperparameter (h) plays the role of the degrees of freedom (e.g., the
% number of averages comprising the estimate). In this routine, we use the
% sample spectral density to create a frequency specific precision matrix
% for the vectorised spectral densities - under the assumption that the
% former of this sample spectral density resembles the predicted spectral
% density (which will become increasingly plausible with convergence).
%
% Camba-Mendez, G., & Kapetanios, G. (2005). Estimating the Rank of the
% Spectral Density Matrix. Journal of Time Series Analysis, 26(1), 37-48.
% doi: 10.1111/j.1467-9892.2005.00389.x
%__________________________________________________________________________
% Copyright (C) 2018 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dcm_csd_Q.m 7751 2019-12-06 11:59:09Z peter $

%-Check for cell arrays
%--------------------------------------------------------------------------
if iscell(csd)
    CSD   = spm_zeros(csd{1});
    n     = numel(csd);
    for i = 1:n
        CSD = CSD + csd{i};
    end
    Q  = spm_dcm_csd_Q(CSD/n);
    Q  = kron(eye(n,n),Q);
    return
end

%-Get precision
%--------------------------------------------------------------------------
SIZ     = size(csd);
Qn      = spm_length(csd);
Q       = spalloc(Qn,Qn,SIZ(1)*prod(SIZ(2:end))^2);
[w,i,j] = ind2sub(SIZ,1:Qn);
for Qi  = 1:Qn
    for Qj = 1:Qn
        if w(Qi) == w(Qj)
            Q(Qi,Qj) = csd(w(Qi),i(Qi),i(Qj))*csd(w(Qi),j(Qi),j(Qj));
        end
    end
end
Q       = inv(Q + norm(Q,1)*speye(size(Q))/32);


%
% References:
%
% C. Lu. A Library of ADMM for Sparse and Low-rank Optimization. National University of Singapore, June 2016.
% https://github.com/canyilu/LibADMM.
% C. Lu, J. Feng, S. Yan, Z. Lin. A Unified Alternating Direction Method of Multipliers by Majorization
% Minimization. IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 40, pp. 527-541, 2018
%

addpath(genpath(cd))
clear

%% Examples for testing the sparse models
% For detailed description of the sparse models, please refer to the Manual.

%% generate toy data
d = 10;
na = 200;
nb = 100;

A = randn(d,na);
X = randn(na,nb);
B = A*X;
b = B(:,1);

opts.tol = 1e-6;
opts.max_iter = 1000;
opts.rho = 1.1;
opts.mu = 1e-4;
opts.max_mu = 1e10;
opts.DEBUG = 0;

%% l1
[X2,obj,err,iter] = l1(A,B,opts);
iter
obj
err
stem(X2(:,1))

%% group l1
g_num = 5;
g_len = round(na/g_num);
for i = 1 : g_num-1
    G{i} = (i-1)*g_len+1 : i*g_len;
end
G{g_num} = (g_num-1)*g_len+1:na;

[X2,obj,err,iter] = groupl1(A,B,G,opts);
iter
obj
err
stem(X2(:,1))

%% elastic net
lambda = 0.01;
[X2,obj,err,iter] = elasticnet(A,B,lambda,opts);
iter
obj
err
stem(X2(:,1))

%% fused Lasso
lambda = 0.01;
[x,obj,err,iter] = fusedl1(A,b,lambda,opts);
iter
obj
err
stem(x)

%% trace Lasso
[x,obj,err,iter] = tracelasso(A,b,opts);
iter
obj
err
stem(x)

%% k-support norm
k = 10;
[X,err,iter] = ksupport(A,B,k,opts);
iter
err
stem(X(:,1));

%% --------------------------------------------------------------

%% regularized l1
lambda = 0.01;
opts.loss = 'l1';
[X,E,obj,err,iter] = l1R(A,B,lambda,opts);
iter
obj
err
stem(X(:,1))

%% regularized group Lasso
g_num = 5;
g_len = round(na/g_num);

for i = 1 : g_num-1
    G{i} = (i-1)*g_len+1 : i*g_len;
end
G{g_num} = (g_num-1)*g_len+1:na;
lambda = 1;
opts.loss = 'l1';
[X,E,obj,err,iter] = groupl1R(A,B,G,lambda,opts);
iter
obj
err
stem(X(:,1))

%% regularized elastic net
lambda1 = 10;
lambda2 = 10;
opts.loss = 'l1';
[X,E,obj,err,iter] = elasticnetR(A,B,lambda1,lambda2,opts);
iter
obj
err
stem(X(:,1))
% stem(E(:,1))

%% regularized fused Lasso
lambda1 = 10;
lambda2 = 10;
opts.loss = 'l1';
[X,E,obj,err,iter] = fusedl1R(A,b,lambda1,lambda2,opts);
iter
obj
err
stem(X(:,1))
stem(E(:,1))

%% regularized trace Lasso
lambda = 0.1;
opts.loss = 'l1';
tic
[x,e,obj,err,iter] = tracelassoR(A,b,lambda,opts);
toc
iter
obj
err
stem(x)

%% regularized k-support norm
lambda = 0.1;
k = 10;
[X,E,err,iter] = ksupportR(A,B,lambda,k,opts);
iter
err
stem(X(:,1));


function [Int, p, mask, w3] = estPolIntGrad(inp, N, logFlag);
% ESTPOLINTGRAD - Estimates and corrects the intensity gradient, using a polynomial model
%
% [Int pol] = estPolIntGrad(inp, N, logFlag);
%
% Inputs:
%  inp - input inplanes affected by the intensity gradient
%  N - order of the polynomial [Nx Ny Nz]
%  logFlag - operates in the logarithm of the intensity
%

if ~exist('N')
   N = [3,3,3];
end
if ~exist('logFlag')
   logFlag = 1;
end

[Ny, Nx, Nz] = size(inp);
inp(find(inp==0)) = NaN;

% chosing appropriate thresholds
II = find(~isnan(inp));
% fitting a GMM
inpMu = mean(inp(II));
inpStd = std(inp(II));
outlim = [min(inp(II)) max(inp(II))];
initCov(1,1,1) = inpStd^2/4;
size(initCov)
[Mu Cov W] = fitGMM(inp(II)', 1, inpMu+2.5*inpStd, initCov, [0.9 0.1], outlim);
%initmu = [inpMu-2*inpStd inpMu+2.5*inpStd];
%initCov(:,:,1:2) = 2*inpStd^2;
%initW = [0.45 0.45 0.1];
%outlim = [min(inp(II)) max(inp(II))];
%[Mu Cov W] = fitGMM(inp(II)', 2, initmu, initCov, initW, outlim);

[hh xx] = hist(inp(II),256);
hh = hh/sum(hh)/(xx(2)-xx(1));
p1 = W(1)*pgauss1d(xx,Mu(1),Cov(:,:,1));
%p2 = W(2)*pgauss1d(xx,Mu(2),Cov(:,:,2));
p3 = W(2)/(max(inp(II))-min(inp(II))) * ones(size(p1));
figure(20); clf
plot(xx,hh,xx,p1+p3)

% chosing as thresholds mu2+-2std2
LoT = Mu(1) - 3*sqrt(Cov(:,:,1))
UpT = Mu(1) + 3*sqrt(Cov(:,:,1))
inp(find( (inp<LoT) | (inp>UpT) )) = NaN;

figure(20)
hold on
stem([LoT UpT], [max(hh),max(hh)]);
hold off

% Change polynomial order to be 2 less than number of non-NaN slices
NzEff = 0;
for k=1:Nz
    tmp = inp(:,:,k);
    if ~all(isnan(tmp(:)))
        NzEff = NzEff + 1;
    end
end
N(3) = min(max(1,NzEff-2), N(3));

% taking logarithm
if logFlag
   inp = log(inp);
end

% selecting valid set of indices
II = find(~isnan(inp));

% building A matrix for the appropriate polynomial order
[x y z] = meshgrid(1:Nx,1:Ny,1:Nz);
A = [];
for m=0:N(1)
  for n=0:max(0,(N(2)-m))
    for o=0:max(0,(N(3)-m-n))
         A = [A x(II).^m .* y(II).^n .* z(II).^o];
      end
   end
end
clear x y z

% robust estimation of the polynomial coefficients
% NOTE - The values of the last two parameters in this function can affect the
% performance of the solution. In general, the recommended value for CB = 4.685
% is very conservative with outliers (almost nothing is considered outlier).
% For this reason I choose a value of 2.5. I also set a bit lower the value of SC
% (1.2 instead of 1.4), so that more outliers are rejected.
[p w]= robustMest(A, inp(II), 3, 1.2);
w3 = zeros(size(inp));
w3(II) = w;

% computation of the intensity correction
il = zeros(Ny,Nx,Nz); k=1;
[x y z] = meshgrid(1:Nx, 1:Ny, 1:Nz);
for m=0:N(1)
  for n=0:max(0,(N(2)-m))
    for o=0:max(0,(N(3)-m-n))
         il = il + p(k)*x.^m.*y.^n.*z.^o;
         k = k+1;
      end
   end
end

if logFlag
   Int = exp(il);
else
   Int = il;
end

mask = zeros(size(inp));
mask(II) = 1;

return

clear
load testdata
[Int, p, mask, w3] = estPolIntGrad(inp, [3 3 1], 1);
inpc = inp./Int;
sinpc = reshape(inpc, [size(inp,1) size(inp,2)*size(inp,3)]);
figure(1)
subplot(4,1,1)
vis(inp)
subplot(4,1,2)
imshow(sinpc, [0.5 1.5], 'notruesize')
subplot(4,1,3)
vis(Int.*mask)
subplot(4,1,4)
vis(w3)
figure(2)
II = find((inpc~=0)&(~isnan(inpc)));
hist(inpc(II),256)


function [kWh] = Nm2kWh(Nm)
% Convert energy or work from newtons-meters to kilowatt-hours.
% Chad A. Greene 2012
kWh = Nm*2.7777777778e-7;


% P(X) = GAUSSIAN(MU0,COV0)
%
% Q(X) = GAUSSIAN(MU,COV)
%
% where the parameters are actually functions
%   MU = MU(THETA),
%   COV = COV(THETA).
%
% More generally, the whole joint distribution Q(X,MU0,COV0) can be a
% function of THETA.
%
% Evaluates KL(Q||P) w.r.t. THETA up to a constant, and the gradient.
%
% vb_rotationcost_gaussian(logdet_Cov, x_invCov0_x, x_invCov0_mu, ...
%                          mu_invCov0_mu, logdet_Cov0, grad_logdet_Cov, ...
%                          grad_x_invCov0_x, grad_x_invCov0_mu, ...
%                          grad_mu_invCov0_mu, grad_logdet_Cov0)
%
% LOGDET_COV         : <LOG(DET(COV))>
% X_INVCOV0_X        : <X'*INV(COV0)*X>
% X_INVCOV0_MU       : <X'*INV(COV0)*MU>
% MU_INVCOV0_MU      : <MU'*INV(COV0)*MU>
% LOGDET_COV0        : <LOG(DET(COV0))>
% GRAD_LOGDET_COV    : D(<LOG(DET(COV))>) / D(THETA)
% GRAD_X_INVCOV0_X   : D(<X'*INV(COV0)*X>) / D(THETA)
% GRAD_X_INVCOV0_MU  : D(<X'*INV(COV0)*MU>) / D(THETA)
% GRAD_MU_INVCOV0_MU : D(<MU'*INV(COV0)*MU>) / D(THETA)
% GRAD_LOGDET_COV0   : D(<LOG(DET(COV0))>) / D(THETA)
%
% where the expectations <...> are taken over the distribution
% Q(X,MU0,COV0). Note that the terms can be evaluated up to a constant
% w.r.t. THETA, which often gives huge savings.

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

function [KL,dKL] = vb_rotationcost_gaussian(logdet_Cov,         ...
                                             x_invCov0_x,        ...
                                             x_invCov0_mu,       ...
                                             mu_invCov0_mu,      ...
                                             logdet_Cov0,        ...
                                             grad_logdet_Cov,    ...
                                             grad_x_invCov0_x,   ...
                                             grad_x_invCov0_mu,  ...
                                             grad_mu_invCov0_mu, ...
                                             grad_logdet_Cov0)

% Cost from <log q(X)>
log_qX = -0.5 * logdet_Cov;
dlog_qX = -0.5 * grad_logdet_Cov;

% Cost from <log p(x)>
log_pX = -0.5 * (logdet_Cov0 + x_invCov0_x - 2*x_invCov0_mu + mu_invCov0_mu);
dlog_pX = -0.5 * (grad_logdet_Cov0 + grad_x_invCov0_x - 2*grad_x_invCov0_mu ...
                  + grad_mu_invCov0_mu);

KL = log_qX - log_pX;
dKL = dlog_qX - dlog_pX;

% $$$ function [f,df_dR] = vb_rotationcost_gaussian(R,logdet_R,invT_R, ...
% $$$                                               XX_R_invCov,X_Mu_invCov)
% $$$
% $$$ [D,N] = size(X);
% $$$
% $$$ % TODO: Check that the sizes match
% $$$
% $$$ % Cost from <log q(X)>
% $$$ log_qX = -N*logdet_R;
% $$$ dlog_qX = -N*invT_R;
% $$$
% $$$ % Cost from <log p(x)>
% $$$ log_pX = -0.5 * (traceprod(XX_R_invCov,R) - 2*traceprod(X_Mu_invCov,R));
% $$$ dlog_pX = X_Mu_invCov - XX_R_invCov;
% $$$
% $$$ f = log_pX -


function spdemovarout
% SPDEMOVAROUT   Sparse interpolation demo for multiple outputs
%    Sparse grid interpolation demo for a function with multiple
%    output parameters and non-vectorized processing. In this case,
%    we use the Maximum-norm-based grid.

% Author : Andreas Klimke, Universitaet Stuttgart
% Date   : January 27, 2004
% Version: 1.3

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

% Define problem dimension
d = 2;

% Define number of output parameters of fvarout
nout = 3;

% Get options structure for sparse interpolation
options = spset('NumberOfOutputs', nout, ...
								'GridType', 'Maximum');

% Create full grid for plotting
gs = 33;
[X,Y] = meshgrid(linspace(0,2,gs),linspace(-1,1,gs));

% Compute sparse grid weights over domain [0,2]x[-1,1]
z = spvals(@fvarout, d, [0 2; -1 1], options, nout);

% Compute inpterpolated values at full grid
for k = 1:nout
	z.selectOutput = k;
	ip{k} = spinterp(z, X, Y);
end

% Plot interpolated results
for k = 1:nout
	subplot(1,nout,k);
	mesh(X, Y, ip{k});
	title(['interpolated out',num2str(k)]);
end

%------------------------------------------------------------------
function [varargout] = fvarout(x,y,nout)
% FVARARGOUT    function with multiple output arguments, the number
% of output arguments is determined by the parameter nout.

out = zeros(1,nout);
out(1:nout) = 1./((x-(1:nout)/nout).^2+(y)^2+(1:nout)./nout);
varargout = num2cell(out);


function y=ar_med_filter(signal,Fs)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function AR_MED_FILTER takes input SIGNAL with Sampling Frequency, %
% Fs, and applies the Yule Walker method based AR filter. The order iof the
% filter is found by Maximum kurtosis. After the application od AR filter,
% the signal is passed through Minimum Entropy Deconvolution. This combined
% AR+MED method brings out the Bearing faults hidden in Noise.
%
% The function plots two figures for AR alone and another for AR+MED
%
% Example:
%   load('s4.mat');
%   signal=s4;
%   Fs=12000;
%   ar_med_filter(signal,Fs);

% The File 's4.mat' is the vibration signal recorded from a OR faulty
% bearing with a sampling frequency of 12000Hz. The Fault frequency is 161
% Hz and is brought out.
%
% This program isa based on the paper:
% Sawalhi N, Randall RB and Endo H (2007) The enhancement of fault detection
% and diagnosis in rolling element bearings using minimum entropy
% deconvolution combined with spectral kurtosis. Mechanical Systems and
% Signal Processing. 21:2616-2633
%
% This function is basically written for Bearing fault diagnosis from
% Vibration signal.
%
%Dont forget to rate or comment on the matlab central site
%http://www.mathworks.in/matlabcentral/fileexchange/authors/258518
%
%Author:Santhana Raj.A
%https://sites.google.com/site/santhanarajarunachalam/
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

sig=signal;

clear y y_e A

for order= 1:100

[A,E]=aryule(sig,order);
y(:,order)=filter(1,A,sig);
kurt(:, order)=kurtosis(y(:,order));

end

[~,index]=sort(kurt,1,'descend');
output=y(:,index(1));

figure();
N=4*2048;T=N/Fs;sig_f=abs(fft(output(1:N),N));
sig_n=sig_f/(norm(sig_f));freq_s=(0:N-1)/T;
plot(freq_s(3:350),sig_n(3:350)); title('AR alone');

%MED
[ar_med f_armed kurt_armed] = med2d(output,30,[],0.01,0);

figure();
N=4*2048;T=N/Fs;sig_f=abs(fft(ar_med(1:N),N));
sig_n=sig_f/(norm(sig_f));freq_s=(0:N-1)/T;
plot(freq_s(3:350),sig_n(3:350)); title('AR +MED');

y=ar_med;

end


% VL_RODR  Rodrigues' formula
%   R = VL_RODR(OM) where OM a 3-dimensional column vector computes the
%   Rodrigues' formula of OM, returning the rotation matrix R =
%   expm(vl_hat(OM)).
%
%   [R,DR] = VL_RODR(OM) computes also the derivative of the Rodrigues
%   formula. In matrix notation this is the expression
%
%           d(vec expm(vl_hat(OM)) )
%     dR = ----------------------.
%                  d om^T
%
%   [R,DR]=VL_RODR(OM) when OM is a 3xK matrix repeats the operation for
%   each column (or equivalently matrix with 3*K elements). In this
%   case R and DR are arrays with K slices, one per rotation.
%
%   See also: VL_IRODR(), VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).


function xp = spm_beta_compare(alpha1,alpha2,Nsamp)
% Compute probability that r1 > r2
% FORMAT xp = spm_beta_compare(alpha1,alpha2,Nsamp)
%
% Input:
% alpha1    - Beta parameters for first density
% alpha2    - Beta parameters for second density
% Nsamp     - number of samples used to compute xp [default = 1e4]
%
% Output:
% xp        - exceedance probability
%
% Compute probability that r1 > r2 where p(r1)=Beta(r1|alpha1),
% p(r2)=Beta(r2|alpha2). Uses sampling.
% Useful for comparing groups in RFX model inference
%__________________________________________________________________________
% Copyright (C) 2009 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_beta_compare.m 3458 2009-10-13 10:05:35Z maria $

if nargin < 3
    Nsamp = 1e4;
end

Nk = length(alpha1);

% Perform sampling in blocks
%--------------------------------------------------------------------------
blk = ceil(Nsamp*Nk*8 / 2^28);
blk = floor(Nsamp/blk * ones(1,blk));
blk(end) = Nsamp - sum(blk(1:end-1));

xp = 0;
for i=1:length(blk)

    % Sample from univariate gamma densities then normalise
    % (see Dirichlet entry in Wikipedia or Ferguson (1973) Ann. Stat. 1,
    % 209-230)
    %----------------------------------------------------------------------
    r1 = zeros(blk(i),Nk);
    for k = 1:Nk
        r1(:,k) = spm_gamrnd(alpha1(k),1,blk(i),1);
    end
    sr1 = sum(r1,2);
    for k = 1:Nk
        r1(:,k) = r1(:,k)./sr1;
    end

    r2 = zeros(blk(i),Nk);
    for k = 1:Nk
        r2(:,k) = spm_gamrnd(alpha2(k),1,blk(i),1);
    end
    sr2 = sum(r2,2);
    for k = 1:Nk
        r2(:,k) = r2(:,k)./sr2;
    end

    % Exceedance probabilities:

    xp = xp + length(find (r1(:,1)>r2(:,1)));

end
xp = xp / Nsamp;


function r = atanh(a)
%ATANH        Hessian (elementwise) inverse hyperbolic tangent
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  K = prod(size(a.x));
  if K==1                   % scalar hessian

    r.x = atanh(a.x);
    f = 1 / ( 1 - sqr(a.x) );
    r.dx = a.dx * f;
    r.hx = ( a.hx + reshape( (a.x*r.dx) * a.dx.' , size(a.hx) ) ) * f;

  else                      % matrix hessian

    N = getappdata(0,'INTLAB_HESSIAN_NUMVAR');
    N2 = N^2;

    r.x = atanh(a.x);
    if issparse(a.hx)               % input sparse

      ax = 1 ./ ( 1 - sqr(full(a.x(:))) );
      sizeax = length(ax);
      [ia,ja,sa] = find(a.dx);
      % check for emptyness: cures Matlab bug
      % a=sparse([],[],[],2,1), [i,j,s]=find(a), s(i).*s(:)  yields error
      if isempty(ia)
        r.dx = sparse([],[],[],N,sizeax);
        r.hx = sparse([],[],[],N2,sizeax);
      else
        adx1 = a.x(:).*ax;
        if isa(a.x,'intval')          % sparse intval
          rdx = times(ax(ja),sa(:),0);
          adx1 = times(adx1(ja),sa(:),0);
          if rdx.complex
            r.dx = intval( sparse(ia,ja,rdx.mid,N,sizeax) , sparse(ia,ja,rdx.rad,N,sizeax) , 'midrad' );
          else
            r.dx = intval( sparse(ia,ja,rdx.inf,N,sizeax) , sparse(ia,ja,rdx.sup,N,sizeax) , 'infsup' );
          end
          if adx1.complex
            adx1 = intval( sparse(ia,ja,adx1.mid,N,sizeax) , sparse(ia,ja,adx1.rad,N,sizeax) , 'midrad' );
          else
            adx1 = intval( sparse(ia,ja,adx1.inf,N,sizeax) , sparse(ia,ja,adx1.sup,N,sizeax) , 'infsup' );
          end
        else                          % sparse point
          r.dx = sparse(ia,ja,ax(ja).*sa(:),N,sizeax);
          adx1 = sparse(ia,ja,adx1(ja).*sa(:),N,sizeax);
        end
        r.hx = adx2rhx(N,sizeax,adx1,r.dx);
      end
      [ia,ja,sa] = find(a.hx);      % sparse point or intval
      % check for emptyness: cures Matlab bug
      % a=sparse([],[],[],2,1), [i,j,s]=find(a), s(i).*s(:)  yields error
      if ~isempty(ia)
        if isa(a.x,'intval')
          rhx = times(ax(ja),sa(:),0);
          if rhx.complex
            r.hx = r.hx + intval( sparse(ia,ja,rhx.mid,N2,sizeax) , sparse(ia,ja,rhx.rad,N2,sizeax) , 'midrad' );
          else
            r.hx = r.hx + intval( sparse(ia,ja,rhx.inf,N2,sizeax) , sparse(ia,ja,rhx.sup,N2,sizeax) , 'infsup' );
          end
        else
          r.hx = r.hx + sparse(ia,ja,ax(ja).*sa(:),N2,sizeax);
        end
      end

    else                            % input full

      r.x = atanh(a.x);
      ax = a.x(:).';
      f = 1 ./ ( 1 - sqr(ax) );
      f = f(ones(N*N,1),:);
      r.dx = a.dx .* f(1:N,:);
      adx = repmat(ax,N,1) .* r.dx;
      r.hx = ( a.hx + adx(repmat(1:N,N,1),:) .* a.dx(repmat(1:N,1,N),:) ) .* f;

    end

  end

  r = class(r,'hessian');

  if rndold
    setround(rndold)
  end


function [pav_trans,score_bounds,llr_bounds] = pav_calibration(tar,non,small_val)
% Creates a calibration transformation function using the PAV algorithm.
% Inputs:
%   tar: A vector of target scores.
%   non: A vector of non-target scores.
%   small_val: An offset to make the transformation function
%     invertible.  small_val is subtracted from the left-hand side
%     of the bin and added to the right-hand side (and the bin
%     height is linear between its left and right ends).
% Outputs:
%   pav_trans: The transformation function.  It takes in scores and
%     outputs (calibrated) log-likelihood ratios.
%   score_bounds: The left and right ends of the line segments
%     that make up the transformation.
%   llr_bounds: The lower and upper ends of the line segments that
%     make up the transformation.

if nargin==0
    test_this()
    return
else
    assert(nargin==3)
    assert(size(tar,1)==1)
    assert(size(non,1)==1)
    assert(length(tar)>0)
    assert(length(non)>0)
end

largeval = 1e6;

scores = [-largeval tar non largeval];
Pideal = [ones(1,length(tar)+1),zeros(1,length(non)+1)];
[scores,perturb] = sort(scores);
Pideal = Pideal(perturb);
[Popt,width,height] = pavx(Pideal);
data_prior = (length(tar)+1)/length(Pideal);
llr = logit(Popt) - logit(data_prior);
bnd_ndx = make_bnd_ndx(width);
score_bounds = scores(bnd_ndx);
llr_bounds = llr(bnd_ndx);
llr_bounds(1:2:end) = llr_bounds(1:2:end) - small_val;
llr_bounds(2:2:end) = llr_bounds(2:2:end) + small_val;
pav_trans = @(s) pav_transform(s,score_bounds,llr_bounds);
end

function scr_out = pav_transform(scr_in,score_bounds,llr_bounds)
scr_out = zeros(1,length(scr_in));
for ii=1:length(scr_in)
    x = scr_in(ii);
    [x1,x2,v1,v2] = get_line_segment_vals(x,score_bounds,llr_bounds);
    scr_out(ii) = (v2 - v1) * (x - x1) / (x2 - x1) + v1;
end
end

function bnd_ndx = make_bnd_ndx(width)
len = length(width)*2;
c = cumsum(width);
bnd_ndx = zeros(1,len);
bnd_ndx(1:2:len) = [1 c(1:end-1)+1];
bnd_ndx(2:2:len) = c;
end

function [x1,x2,v1,v2] = get_line_segment_vals(x,score_bounds,llr_bounds)
p = find(x>=score_bounds,1,'last');
x1 = score_bounds(p);
x2 = score_bounds(p+1);
v1 = llr_bounds(p);
v2 = llr_bounds(p+1);
end

function test_this()
ntar = 10;
nnon = 12;
tar = 2*randn(1,ntar)+2;
non = 2*randn(1,nnon)-2;
tarnon = [tar non];

scores = [-inf tarnon inf];
Pideal = [ones(1,length(tar)+1),zeros(1,length(non)+1)];
[scores,perturb] = sort(scores);
Pideal = Pideal(perturb);
[Popt,width,height] = pavx(Pideal);
data_prior = (length(tar)+1)/length(Pideal);
llr = logit(Popt) - logit(data_prior);
[dummy,pinv] = sort(perturb);
tmp = llr(pinv);
llr = tmp(2:end-1)

pav_trans = pav_calibration(tar,non,0);
pav_trans(tarnon)

end


function h=ref_pconv(f,g,ctype)
%REF_PCONV  Reference PCONV
%   Usage:  h=ref_pconv(f,g)
%
%   PCONV(f,g) computes the periodic convolution of f and g.

% AUTHOR: Peter L. Søndergaard

L=length(f);
h=zeros(L,1);

switch(lower(ctype))
  case {'default'}
    for ii=0:L-1
      for jj=0:L-1
	h(ii+1)=h(ii+1)+f(jj+1)*g(mod(ii-jj,L)+1);
      end;
    end;
  case {'r'}
    for ii=0:L-1
      for jj=0:L-1
	h(ii+1)=h(ii+1)+f(jj+1)*conj(g(mod(jj-ii,L)+1));
      end;
    end;
  case {'rr'}
    for ii=0:L-1
      for jj=0:L-1
	h(ii+1)=h(ii+1)+conj(f(mod(-jj,L)+1))*conj(g(mod(jj-ii,L)+1));
      end;
    end;
end;


function [ J, grad ] = cost_function ( x, e_conn, u, q, M, lam, c )

%*****************************************************************************80
%
%% COST_FUNCTION evaluate the cost function and its gradient.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2011
%
%  Author:
%
%    Jeff Borggaard, John Burkardt, Catalin Trenchea, Clayton Webster
%
%  Parameters:
%
%    Input, real X(N_NODES,1), the nodes of the mesh.
%
%    Input, integer E_CONN(N_ELEMENTS,NEL_DOF),
%
%    Input, real U(N_NODES), the value of the finite element solution at
%    each node.
%
%    Input, real Q(N_NODES,1), the value of the control function at
%    each node of the mesh.
%
%    Input, real M(N_NODES,N_NODES), the mass matrix.
%    M(I,J) = Integral PHI(I) PHI(J).
%
%    Input, real LAM(N_NODES), the value of the adjoint variable at each node.
%
%    Input, real C(N_EQUATIONS), the value of the adjoint equation right hand
%    side at each internal node.
%
%    Output, real J, the cost function, 1/2 sqrt ( integral ( U - U_HAT )^2 ).
%
%    Output, real GRAD(N_NODES,1), the gradient of the cost function (?).
%
%  Local Parameters:
%
%    Local, real ALPHA, a weighting factor in the computation of GRAD.
%
%    Local, real GRAD_LOCAL(NEL_DOF), the local contribution to the gradient
%    at the nodes of a given element.
%
%    Local, real LAMX_G(N_GAUSS), the dLAMdX function at the Gauss points
%    in a given element.
%
%    Local, integer N_ELEMENTS, the number of elements.
%
%    Local, integer N_GAUSS, the number of Gauss points.
%
%    Local, integer N_NODES, the number of nodes.
%
%    Local, integer NEL_DOF, the number of degrees of freedom per element.
%
%    Local, integer NODES_LOCAL, the indices of the nodes in a given element.
%
%    Local, real P_X, the derivative of the basis function evaluated at
%    the Gauss points in a given element.
%
%    Local, real PHI, the basis function evaluated at
%    the Gauss abscissas in a given element.
%
%    Local, real QX_G(N_GAUSS), the value of dQdX at the Gauss points in a
%    given element.
%
%    Local, real R(N_GAUSS), the Gauss points.
%
%    Local, real UX_G(N_GAUSS), the dUdX function at the Gauss points in a
%    given element.
%
%    Local, real W(N_GAUSS), the Gauss weights.
%
%    Local, real W_G(N_GAUSS), the Gauss weights mapped to a given element.
%
%    Local, real X_G(N_GAUSS), the Gauss points mapped to a given element.
%
%    Local, real X_LOCAL(NEL_DOF), the nodes in a given element.
%
  [ n_nodes, n_dimensions ] = size ( x );
  [ n_elements, nel_dof ] = size ( e_conn );
  n_gauss = 3;
  [ r, w ] = oned_gauss ( n_gauss );

  J = 0.5 * sqrt ( c' * M(2:end-1,2:end-1) * c );
%
%  Compute the gradient.
%
  grad = zeros(n_nodes,1);

  alpha = 0.000003;

  for n_el = 1 : n_elements

    nodes_local = e_conn(n_el,:);
    x_local = x(nodes_local,:);
    [ x_g, w_g, phi, p_x ] = oned_shape ( x_local, r, w );

    ux_g = p_x * u(nodes_local);
    qx_g = p_x * q(nodes_local);
    lamx_g = p_x * lam(nodes_local);

    grad_local = alpha * oned_f_int ( qx_g,           p_x, w_g ) ...
                       - oned_f_int ( lamx_g .* ux_g, phi, w_g );
%
%  Assemble local contribution to gradient.
%
    for n_t = 1 : nel_dof
      n_test = nodes_local(n_t);
      grad(n_test) = grad(n_test) + grad_local(n_t);
    end

  end

  return
end


function [u,v,w]=v_sphrharm(m,a,b,c,d)
%V_SPHRHARM  forward and inverse spherical harmonic transform
%
% Usage: (1) y=('f',n,x)      % Calculate complex transform of spatial data x up to order n
%
%        (2) y=('fr',n,x)     % Calculate real transform of spatial data x(ne,na) up to order n
%                             % x is specified on a uniform grid with inclination e=(0.5:ne-0.5)*pi/ne
%                             % and azimuth a=(0:na-1)*2*pi/na. The North pole has inclination 0 and
%                             % azimuths increase going East.
%
%        (3) y=('fd',n,e,a,v) % Calculate transform of impulse at (e(i),a(i)) of height v(i) up to order n
%                             % e(i), a(i) and v(i) should be the same dimension; v defaults to 1.
%
%        (4) x=('i',y,ne,na)  % Calculate spatial data from spherical harmonics y on
%                             % a uniform grid with ne inclinations and na azimuths
%
%        (5) [e,a,w]=('cg',ne,na)   % Calculate the inclinations, azimuths and
%                                   % quadrature weights of a Gaussian sampling grid
%
%        (6) n=2;             % illustrate real basis functions upto order 2
%            for i=0:n
%              for j=-i:i
%                subplot(n+1,2*n+1,i*(2*n+1)+j+n+1);
%                v_sphrharm('irp',[zeros(1,i^2+i+j) 1],25,25);
%              end
%            end
%
%  Inputs:  m string specifying various options:
%               'f' forward transform
%               'i' inverse transform
%               'c' calculate the coordinates of the sampling grid [default if f or i omitted]
%               'r' real spherical harmonics instead of complex
%               'u' uniform inclination grid:  e=(0.5:ne-0.5)*pi/ne (includes neither pole) [default]
%                      for invertibility, ne>=2N+1 for order N
%               'U' uniform inclination grid:  e=(0:ne-1)*pi/ne (includes North pole only)
%                      for invertibility, ne>=2N+1 for order N
%               'g' gaussian inclination grid (non-uniform but fewer samples needed)
%                      for invertibility, ne>=N+1 for order N
%               'a' arbitrary (specified by user - inverse transform only)
%               'd' delta function [forward transform only]
%               'p' plot result
%               'P' plot with colourbar
%
%           The remaining inputs depend on the mode specified:
%               'f'  a         order of transform
%                    b(ne,na)  input data array on the chosen inclination-azimuth sampling grid
%               'fd' a         order of transform
%                    b(k)      inclinations of k delta functions
%                    c(k)      azimuths of k delta functions
%                    d(k)      amplitudes of k delta functions [default=1]
%               'i'  a()       spherical harmonics as a single row vector in the order:
%                                 (0,0),(1,-1),(1,0),(1,1),(2,-2),(2,-1),...
%                              To access as a 2-D harmonic: Y(n,m)=a(n^2+n+m+1) where n=0:N, m=-n:n
%                    b         number of inclination values in output grid
%                    c         number of azimuth values in output grid
%               'c'  a         number of inclination values in output grid
%                    b         number of azimuth values in output grid
%
% Outputs:  u output data according to transform direction:
%                'f': spherical harmonics as a single row vector in the order:
%                        (0,0),(1,-1),(1,0),(1,1),(2,-2),(2,-1),...
%                     To access as a 2-D harmonic: Y(n,m)=u(n^2+n+m+1) where n=0:N, m=-n:n
%                'i': u(ne,na) is spatial data sampled on an azimuth-inclination grid
%                     with ne inclination points (in 0:pi) and na azimuth points (in 0:2*pi)
%                'c': u(ne) gives inclination grid positions with 0 being the North pole
%           v(na) gives azimuth grid positions
%           w(ne) gives the quadrature weights used in the transform
%
% Suppose f(e,a) is a complex-valued function defined on the surface of the
% sphere (0<=e<=pi, 0<=a<2pi) where e=inclination=pi/2-elevation and
% a=azimuth. (0,*) is the North pole, (pi/2,0) is on the equator near
% Ghana and (pi/2,pi/2) is on the equator near India.
%
% We can approximate f(e,a) using complex spherical harmonics as
% f(e,a) = sum(F(n,m)*u(n,m,e,a)) where the sum
% is over n=0:N,m=-n:n giving a total of (N+1)^2 coefficients F(n,m).
%
% If f(e,a) happens to be real-valued, then we can transform the
% coefficients using G(n,0)=F(n,0) and, for m>0,
% [G(n,m); G(n,-m)]=sqrt(0.5)*[1 1; 1i -1i]*[F(n,m); F(n,-m)]
% to give the real spherical harmonic coefficients G(n,m).
%
% The basis functions u(n,m,e,a) are orthonormal under the inner product
% <u(e,a),v(e,a)> = integral(u(e,a)*v'(e,a)*sin(e)*de*da).
%
%  Minimum spatial grid for invertibility:
%     Gaussian grid: ne >= N+1,  na >= 2N+1
%     Uniform grid:  ne >= 2N+1, na >= 2N+1
%     An inverse transform followed by a forward transform will restore the
%     original coefficient values provided that the sampling grid is large
%     enough. The advantage of the Gaussian grid is that it can be smaller.
%
%   Data formats:
%     (1) Spatial Data: x=v_sphrharm('i',y,ne,na)
%            x(1:ne,1:na) is spatial data sampled on an azimuth-inclination grid
%                         with ne inclination points (in 0:pi) and
%                         na azimuth points (in 0:2*pi)
%     (2) Spherical harmonics: y=v_sphrharm('f',n,x)
%            y(1:(n+1)^2)  spherical harmonics as a single row vector
%                          in the order: (0,0),(1,-1),(1,0),(1,1),(2,-2),(2,-1),...
%                          To access as a 2-D harmonic, use
%                          Y(n,m)=y(n^2+n+m+1)   where n=0:N, m=-i:i
%     (3) Sampling Grid:  [e,a,w]=v_sphrharm('c',ne,na)
%            e(1:ne)   monotonically increasing inclination values (0<=e<=pi) where 0 is the North pole
%            a(1:na)   monotonically increasing azimuth values (0<=a<2pi)
%            w(1:ne)   Quadrature weights
%

% future options
%    direction: [m=v_rotation matrix]
%    transform: [h=complex harmonics but only the positive ones specified]
%    grid       d=delta function [forward transform only]
%               [s=sparse azimuth]
%               [z=include both north and south pole]
%
% bugs:
%   (1) we could save space and time by taking advantage of symmetry of legendre polynomials
%   (2) the number of points in the inclination (elevation) grid must, for now, be even if the 'U' option is chosen
%   (3) should ensure that the negative nyquist azimuth frequency is not used
%   (4) save time by manipulating only the necessary 2*m columns of the da matrix
%   (5) should make non-existant coefficients black in plots
%   (6) using 'surf' for plots adds an offset in azimuth and elevation
%       because colours correspond to vertices not faces
%   (7) the normalization for mode 'fd' seems incorrect
%   (8) mode 'fd' should allow multiple impulses to be summed

% errors:

% tests:
% check for inverse transform n=4; m=4; [ve,va]=spvals(8,8,n,m); ve*va-v_sphrharm('iur',[zeros(1,n^2+n+m) 1],8,8)
% check inverse followed by forward: no=4; h=rand(1,(no+1)^2); max(abs(v_sphrharm('fur',v_sphrharm('iur',h,10,10),no)-h))
% same but complex: no=4; h=rand(1,(no+1)^2); max(abs(v_sphrharm('fu',v_sphrharm('iu',h,10,10),no)-h))
% same but gaussian grid: no=4; h=rand(1,(no+1)^2); max(abs(v_sphrharm('fg',v_sphrharm('ig',h,6,10),no)-h))

%      Copyright (C) Mike Brookes 2009
%      Version: $Id: v_sphrharm.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% decode option string
mv='c u ';       % mv(1)=transform, mv(2)=real/complex, mv(3)=grid, mv(4)=plot
if ~nargout
    mv(4)='P';
end
mc='ficruUgadpP';
mi=[1 1 1 2 3 3 3 3 3 4 4];
for i=1:length(m)
    j=find(m(i)==mc);
    if ~isempty(j)
        mv(mi(j))=m(i);
    end
end

switch mv(1)
    case 'f' % forward transform [sp]=('f',order,data)
        if mv(3)~='d'
            % input data has elevations down the columns and azimuths along the rows
            % output data is in the form:
            [ne,na]=size(b);
            da=fft(b,[],2)*sqrt(pi)/na;
            if mv(2)=='r' % only actually need to do this for min(a,floor(na/2)) values (but nyquist is tricky)
                ix=2:ceil(na/2);
                iy=na:-1:na+2-ix(end);
                da(:,ix)=da(:,ix)+da(:,iy);
                da(:,iy)=1i*(da(:,ix)-2*da(:,iy));  % note
                da(:,1)=da(:,1)*sqrt(2);
            else
                da=da*sqrt(2);
            end
            [ue,we,lgp]=sphrharp(mv(3),ne,a);
            da=da.*repmat(we',1,na);
            u=zeros(1,(a+1)^2);
            i=0;
            for nn=0:a
                % we could vectorize this to avoid the inner loop
                % we should ensure the the negative nyquist value is not used
                for mm=-nn:nn
                    i=i+1;
                    u(i)=lgp(1+nn*(nn+1)/2+abs(mm),:)*da(:,1+mod(mm,na));
                end
            end
        else       % forward transform of impulses [sp]=('fd',order,e,a,value)
            if nargin<5
                d=ones(size(b));
            end
            [ue,we,lgp]=sphrharp('a',b(:),a);
            uu=zeros(1,(a+1)^2);     % reserve space for spherical harmonic coefficients
            u=uu;
            for j=1:numel(b)
                i=0;
                if mv(2)=='r'
                    for nn=0:a
                        % we could vectorize this to avoid the inner loop
                        % we should ensure the the negative nyquist value is not used
                        i=i+1;
                        uu(i+nn)=lgp(1+nn*(nn+1)/2,1);
                        for mm=1:nn
                            uu(i+nn-mm)=lgp(1+nn*(nn+1)/2+abs(mm),j)*sin(mm*c(j))*sqrt(2);
                            uu(i+nn+mm)=lgp(1+nn*(nn+1)/2+abs(mm),j)*cos(mm*c(j))*sqrt(2);
                        end
                        i=i+2*nn;
                    end
                else
                    for nn=0:a
                        % we could vectorize this to avoid the inner loop
                        % we should ensure the the negative nyquist value is not used
                        for mm=-nn:nn
                            i=i+1;
                            uu(i)=lgp(1+nn*(nn+1)/2+abs(mm),j)*exp(-1i*mm*c(j));
                        end
                    end
                end
                u=u+d(j)*uu/sqrt(2*pi);
            end
        end
    case 'i' % inverse transform [data]=('i',sp,nincl,nazim)
        %or [data]=('a',sp,e,a) where e is a list of elevations and a is either a corresponding list of azimuths
        % or else a cell array contining several azimuths for each inclination
        % input data is sp=[(0,0) (1,-1) (1,0) (1,1) (2,-2) (2,-1) (2,0) ... ]
        % length is (n+1)^2 where n is the order
        % output data is an array [ne,na]
        nsp=ceil(sqrt(length(a)))-1;
        [ue,we,lgp]=sphrharp(mv(3),b,nsp);
        if mv(3)=='a'
            na=nsp*2+1;
            ne=length(b);
        else
            na=c;
            ne=b;
        end
        i=0;
        da=zeros(ne,na);
        for nn=0:nsp
            % this could be vectorized somewhat to speed it up
            for mm=-nn:nn
                i=i+1;
                if i>length(a)
                    break;
                end
                da(:,1+mod(mm,na))=da(:,1+mod(mm,na))+a(i)*lgp(1+nn*(nn+1)/2+abs(mm),:)';
            end
        end
        if na>1 && mv(2)=='r' % convert real to complex only actually need to do this for min(b,floor(na/2)) values (but nyquist is a bit tricky)
            ix=2:ceil(na/2);
            iy=na:-1:na+2-ix(end);
            da(:,iy)=(da(:,ix)+1i*da(:,iy))*0.5;
            da(:,ix)=da(:,ix)-da(:,iy);
            da(:,1)=da(:,1)/sqrt(2);
        else
            da=da/sqrt(2);
        end
        if mv(3)=='a' % do a slow fft
            if iscell(c)
                u{ne,1}=[]; % reserve space for output cell array
                for i=1:ne
                    ai=c{i};
                    ui=repmat(da(i,1),1,length(ai));
                    for j=1:nsp
                        exj=exp(1i*j*ai(:).'); % we could vectorize this more
                        ui=ui+da(i,j+1).'.*exj+da(i,na+1-j).'.*conj(exj);
                    end
                    u{i}=ui/sqrt(pi);
                end
            else
                u=da(:,1);
                for j=1:nsp
                    exj=exp(1i*j*c(:)); % we could vectorize this more
                    u=u+da(:,j+1).*exj+da(:,na+1-j).*conj(exj);
                end
                u=u/sqrt(pi);
                if mv(2)=='r' && isreal(a)
                    u=real(u);
                end
            end
        else
            u=ifft(da,[],2)*na/sqrt(pi); % could put the scale factor 1/sqrt(pi) earlier
            if mv(2)=='r' && isreal(a)
                u=real(u);
            end
        end
    case 'c' % just output coordinates [inclination,azim,weights]=('c',nincl,nazim)
        % if m!='g', then order, a, must be even
        [u,w]=sphrharp(mv(3),a,0);
        if nargin<3
            b=ceil(a/1+(mv(3)=='g'));
        end
        v=(0:b-1)*2*pi/b;
end
if mv(4)~=' '
    switch mv(1)
        case 'f'
            if mv(2)=='r'
                ua=u;
                tit='Real Coefficients';
            else
                ua=abs(u);
                tit='Complex Coefficient Magnitudes';
            end
            nu=length(ua);
            no=ceil(sqrt(nu))-1;
            yi=zeros(no,2*no+1);
            for i=0:no
                for j=-i:i
                    iy=i^2+i+j+1;
                    if iy<=nu
                        yi(i+1,j+no+1)=ua(iy);
                    end
                end
            end
            imagesc(-no:no,0:no,yi);
            axis 'xy';
            if mv(4)=='P'
                colorbar;
            end
            xlabel('Harmonic');
            ylabel('Order');
            title(tit);
        case 'i'            % [data]=('i',sp,nincl,nazim)
            vv=(0:na)*2*pi/na;  % azimuth array
            gr=sin(ue)';
            surf(gr*cos(vv),gr*sin(vv),repmat(cos(ue)',1,na+1),real(u(:,[1:na 1])));
            axis equal;
            xlabel('X');
            ylabel('Y');
            zlabel('Z');
            if mv(4)=='P'
                colorbar;
            end
            %             v_cblabel('Legendre Weight');
            %             title(sprintf('Sampling Grid: %s, %d, %d',mv(3),length(u),na-1));
        case 'c'            % [inclination,azim,weights]=('c',nincl,nazim)
            vv=[v v(1)];    % replicate initial azimuth point
            na=length(vv);
            gr=sin(u)';
            mesh(gr*cos(vv),gr*sin(vv),repmat(cos(u)',1,na),repmat(w',1,na));
            axis equal;
            xlabel('X');
            ylabel('Y');
            zlabel('Z');
            if mv(4)=='P'
                colorbar;
                v_cblabel('Quadrature Weight');
            end
            title(sprintf('Sampling Grid: %s, %d, %d',mv(3),length(u),na-1));
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [ueo,weo,lgpo]=sphrharp(gr,ne,nsp)
% calculate persistent variables for grid points and legendre polynomials
% we recalculate if transform order or inclination grid size or type are changed
% gr = grid type:
%       'u'=uniform inclination starting at pi/2n (default)
%       'U'=uniform but starting at north pole
%       'g'=gaussian inclination
%       'a'=arbitrary (specified by user - inverse transform only)
% ne = number of inclination values
% nsp = maximum order
% Outputs:
%    ueo(ne)     vector containing the inclination values
%    weo(ne)     vector containing the quadrature weights
%    lgpo((nsp+1)*(nsp+2)/2,ne)  evaluates the Schmidt seminormalized
%                                associated Legendre function at each value
%                                of cos(ue) for n=0:nsp, m=0:n.
persistent gr0 lgp ne0 ue we nsp0
if isempty(ne0)
    ne0=-1;
    nsp0=-1;
    gr0='a';
end
if gr=='a'
    ue=ne;
    ne=length(ue);
    ne0=-1;
    gr0=gr;
    nsp0=-1; % delete previous legendre polynomials
    lgp=[];
    we=[];
elseif gr~=gr0 || ne~=ne0
    if gr=='g'
        r = 1:ne-1;
        r = r ./ sqrt(4*r.^2 - 1);
        p = zeros( ne, ne );
        p( 2 : ne+1 : ne*(ne-1) ) = r;
        p( ne+1 : ne+1 : ne^2-1 ) = r;
        [q, ue] = eig(p);
        [ue, k] = sort(diag(ue));
        ue=acos(-ue)';
        we = 2*q(1,k).^2;
    elseif gr=='U'
        if rem(ne,2)
            error('inclination grid size must be even when using ''U'' option');
        end
        ue=(0:ne-1)*pi/ne;
        xx=zeros(1,ne);
        ah=ne/2;
        xx(1:ah)=(1:2:ne).^(-1);
        we=-4*sin(ue).*imag(fft(xx).*exp(-1i*ue))/ne;
    else % default is m='u'
        ue=(1:2:2*ne)*pi*0.5/ne; % vector of elevations
        vq=(ne-abs(ne+1-2*(1:ne))).^(-1).*exp(-1i*(ue+0.5*pi));
        we=(-2*sin(ue).*real(fft(vq).*exp(-1i*(0:ne-1)*pi/ne))/ne);
    end
    gr0=gr;
    ne0=ne;
    nsp0=-1; % delete previous legendre polynomials
    lgp=[];
end
if nsp>nsp0
    lgp((nsp+1)*(nsp+2)/2,ne)=0; % reserve space
    for i=nsp0+1:nsp
        lgp(1+i*(i+1)/2:(i+1)*(i+2)/2,:)=legendre(i,cos(ue),'sch')*sqrt(0.5*i+0.25);
        lgp(1+i*(i+1)/2,:)=lgp(1+i*(i+1)/2,:)*sqrt(2);
    end
    nsp0=nsp;
end
lgpo=lgp;
weo=we;
ueo=ue;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   FANUC LR MATE 10iA, FANUC Robotics Europe.
%   Author: David Garcia Munyoz, Nestor Gomez Lopez, Teresa Pomares
%   Palorames.
%   Universidad Miguel Hernandez de Elche.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function q = inversekinematic_fanuc_mate(robot, T)
%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

%Evaluate the parameters
d = eval(robot.DH.d);

%See geometry at the reference for this robot
L6=d(6);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%Arrange solutions, there are 8 possible solutions so far.
% if q1 is a solution, q1* = q1 + pi is also a solution.
% For each (q1, q1*) there are two possible solutions
% for q2 and q3 (namely, elbow up and elbow up solutions)
% So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
% there exist two more possible solutions for the last three joints, generally
% called wrist up and wrist down solutions. For this reason,
%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));
% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);

A3=a(3); % desfase

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Eslab�n equivalente para simplificar el desfase
l3 = sqrt (A3^2 + L3^2);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = (acos((L2^2+r^2-l3^2)/(2*r*L2)));

if ~isreal(gamma)
    disp('WARNING:inversekinematic_fanuc_mate: the point is not reachable for this configuration, imaginary solutions');
    %gamma = real(gamma);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);

A3= a(3); %desfase

%See geometry of the robot
%compute L4
l3 = sqrt(A3^2 + L3^2);

%the angle phi is fixed
phi=acos((A3^2+l3^2-L3^2)/(2*A3*l3));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = (acos((L2^2 + l3^2 - r^2)/(2*L2*l3)));

if ~isreal(eta)
   disp('WARNING:inversekinematic_fanuc_mate: the point is not reachable for this configuration, imaginary solutions');
   %eta = real(eta);
end

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - eta;
q3(2) = pi - phi + eta;

%remove complex solutions for q for the q1+pi solutions
function  qreal = arrange_solutions(q)
qreal=q(:,1:4);

%sum along rows if any angle is complex, for any possible solutions, then v(i) is complex
v = sum(q, 1);

for i=5:8,
    if isreal(v(i))
        qreal=[qreal q(:,i)]; %store the real solutions
    end
end

qreal = real(qreal);


% Fig. 6.42   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

clear all;
close all;

alpha=logspace(-2,0,100);
N=size(alpha);
ialpha=ones(N)./alpha;
dum=(ialpha-ones(N))./(ialpha+ones(N));
phimax=asin(dum);
ff=180/pi;
phimax=ff*phimax;
semilogx(ialpha,phimax);
xlabel('1/\alpha');
ylabel('Maximum phase lead (deg)');
grid;
title('Fig. 6.53 Maximum phase increase for lead compensation.');


% div = GRAPH_DIV(S, A, ki, kj)
%
%Divergence of signal residing on edges of a graph. The output resides on
%nodes. The result should be such that
%   graph_div(graph_grad(s)) = L * s,
%where L is the graph Laplacian associated with the adjacency matrix A.
%
%OUTPUT:
%   div: divergence of columns of S defined on vertices
%
%INPUTS:
%   S: signal defined on edges e
%   A: adjacency matrix (weighted or not)
%   ki, kj: indices of edges by graph_adj2vec
%
%
% TODO: does not handle normalized Laplacian case!!
%
%
%note that the norm of the operator is
%
% ||D||^2_2 = ||L||_2, where G is the divergence operator and L is the
% Laplacian used.
%
%
%see also: graph_grad, graph_adj2vec, sgwt_laplacian
%
%code author: Vassilis Kalofolias
%date: Aug 2013

function div = gsp_div_old(G,s)

if ~strcmp(G.lap_type,'combinatorial')
    error('Not implemented yet. However ask Nathanael it is very easy');
end

if size(s,1) ~= G.Ne
    error('Signal size not equal to number of edges');
end

k = size(s,2);

% What happens with loops? (diagonal elements of G, A)
%
% I think both div and grad is zero on the diagonal elements. But then is
% it consistent with the Laplacian? (it has to be div(grad(s)) = 2*L*s) for
% a signal s defined on the vertices.

div = zeros(G.N, k);

for ii = 1 : k
    tmp = sparse(G.v_in, G.v_out, s(:,ii), G.N,G.N);
    tmp = tril(tmp,1) - tril(tmp,1)';

    % the divergence
    div(:,ii) = sum(tmp.* sqrt(G.W), 2);
end

end


function vos_map = obtain_vos_map(img_q,N)
    % build color cube (color histogram)
    [row,col] = size(img_q(:,:,1));
    color_cube = zeros(N,N,N);
    for i = 1 : row
        for j = 1 : col
            cr = img_q(i,j,1);
            cg = img_q(i,j,2);
            cb = img_q(i,j,3);
            color_cube(cr,cg,cb) = color_cube(cr,cg,cb) + 1;
        end
    end

    % label major color
    num_counted_pixels = 0; % record numder of pixels that have been counted
    num_major_color = 0; % record numder of major colors
    prop = 0.95; % care about 95%
    while num_counted_pixels < prop*row*col
        color_most_n = max(max(max(color_cube))); % number of the most color at this iteration
        same_n = length(color_cube(color_cube == color_most_n));
        num_counted_pixels = num_counted_pixels + same_n*color_most_n;
        num_major_color = num_major_color + same_n;
        % label the counted points in color_cube as -1, meaning that these
        % colors are "major"
        color_cube(color_cube == color_most_n) = -1;
    end

    % assign the rest 5% colors to their near major color
    [r_set,g_set,b_set] = ind2sub(size(color_cube),find(color_cube==-1));
    while ~isempty(color_cube(color_cube>0))
        color_most_n = max(max(max(color_cube)));
        [cr,cg,cb] = ind2sub(size(color_cube),find(color_cube==color_most_n));
        color_cube(color_cube==color_most_n) = -2;
        for i = 1 : length(cr)
            dist = zeros(length(r_set),1);
            for j = 1 : length(r_set)
                dist(j) = (cr(i)-r_set(j))^2 + (cg(i)-g_set(j))^2 + (cb(i)-b_set(j))^2;
            end
            label = find(dist == min(dist));
            r_assign = r_set(label(1));
            g_assign = g_set(label(1));
            b_assign = b_set(label(1));
            for m = 1 : row
                for n = 1 : col
                    if img_q(m,n,1) == cr(i) && img_q(m,n,2)==cg(i) && img_q(m,n,3)==cb(i)
                        img_q(m,n,:) = reshape([r_assign,g_assign,b_assign],1,1,3);
                    end
                end
            end
        end
    end
    % compute vos saliency
    [r_set,g_set,b_set] = ind2sub(size(color_cube),find(color_cube==-1));
    sal = zeros(length(r_set),1);
    vos_map = zeros(row,col);
    for i = 1 : length(r_set)
        for j = 1 : length(r_set)
            if j == i
                continue
            end
            sal(i) = sal(i) + sqrt((r_set(i)-r_set(j))^2+(g_set(i)-g_set(j))^2+(b_set(i)-b_set(j))^2);
        end
        for m = 1 : row
            for n = 1 : col
                if img_q(m,n,1)==r_set(i) && img_q(m,n,2)==g_set(i) && img_q(m,n,3)==b_set(i)
                    vos_map(m,n) = sal(i);
                end
            end
        end
    end
    vos_map = mat2gray(vos_map);
end


function imgOut = DurandTMO(img, target_contrast, filter_type)
%
%       imgOut = DurandTMO(img, target_contrast, filter_type)
%
%
%        Input:
%           -img: input HDR image
%           -target_contrast: how to reduce the dynamic range
%
%        Output:
%           -imgOut: tone mapped image
%
%     Copyright (C) 2010-15  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/>.
%
%     The paper describing this technique is:
%     "Fast Bilateral Filtering for the Display of High-Dynamic-Range Images"
% 	  by Fredo Durand and Julie Dorsey
%     in Proceedings of SIGGRAPH 2002
%

%is it a three color channels image?
check13Color(img);

checkNegative(img);

%default parameters
if(~exist('target_contrast', 'var'))
    target_contrast = 5; %as in the original paper
end

if(~exist('filter_type', 'var'))
    filter_type = 'approx_importance';
end

%compute luminance channel
L = lum(img);

sigma_s = max(size(L)) * 0.02;

%separate detail and base
[Lbase, Ldetail] = bilateralSeparation(L, sigma_s, 0.4, 'log10', filter_type);

eps = 1e-6;
log_base = log10(Lbase + eps);
max_log_base = max(log_base(:));
log_detail = log10(Ldetail);
c_factor = log10(target_contrast) / (max_log_base - min(log_base(:)));
log_absolute = c_factor * max_log_base;

log_Ld = log_base * c_factor + log_detail  - log_absolute;
Ld = 10.^(log_Ld) - eps;
Ld(Ld < 0.0) = 0.0;

%change luminance
imgOut = ChangeLuminance(img, L, Ld);

end