% fig2d size histogram for all clusters within one fish

% load all clusters (Fish6_woA_Master0.7)
hfig = figure;
InitializeAppData(hfig);
ResetDisplayParams(hfig);
i_fish = 6;
[cIX,gIX,M] = LoadSingleFishDefault(i_fish,hfig);

%% fig4c: Distribution of cluster sizes
U = unique(gIX);
numU = length(U);
C = zeros(numU,1);
for i=1:numU,
    IX = find(gIX == U(i));
    C(i) = length(IX);
end
% [N,edges] = histcounts(C,10:10:2100);
%
% figure('Position',[500,500,350,300]);hold on;
% for i = 1:length(N),
%     plot([edges(i),edges(i)],[0,N(i)],'color',[1,0.5,0.5],'linewidth',6)
% end
% set(gca,'XScale','log')

% try making log-scaled bins for the histogram
% bins = 10:10:2100; % enough for Autoclus0.7
bins = 10:100:10700;
logbins = log(bins);
linbins = linspace(logbins(1),logbins(end),10);
xbins = exp(linbins);

[N,edges,bin] = histcounts(C,xbins);
%
figure('Position',[500,500,200,200]);hold on; % [500,500,350,300]
for i = 1:length(N),
    plot([edges(i),edges(i)],[0,N(i)],'color',[1,0.5,0.5],'linewidth',8)
end
% bar(edges(1:length(N)),N)
set(gca,'XScale','log')
xlim([5,10^4])
set(gca,'XTick',[10,100,1000,10000])
% ylim([0,70])
xlabel('cluster size')
ylabel('count')

%% fig4d: Distribution of within-cluster correlations
U = unique(gIX);
numU = length(U);
B = zeros(numU,3);
for i=1:numU,
    IX = find(gIX == U(i));
    coeffs = corr(M(IX,:)');
    m = coeffs(:);
%     B(i,1) = min(m);
    B(i,2) = mean(m);
%     B(i,3) = median(m);
end

figure('Position',[500,500,250,150]);
hist(B(:,2),0:0.05:1)
h = findobj(gca,'Type','patch');
h.FaceColor = [0.5 0.5 0.5];
h.EdgeColor = 'w';
xlim([0,1])
set(gca,'XTick', 0:0.2:1);
xlabel('average corr. within cluster')
ylabel('count')

%% fig4a... kmeans
i_ClusGroup = 2;% 3;
i_Cluster = 8; % 1;
[cIX,gIX] = LoadCluster_Direct(i_fish,i_ClusGroup,i_Cluster);
M = UpdateIndices_Manual(hfig,cIX,gIX,numU);

%% left plot
figure('Position',[50,100,800,1000]);
% isCentroid,isPlotLines,isPlotBehavior,isPlotRegWithTS
setappdata(hfig,'isPlotBehavior',1);
setappdata(hfig,'isStimAvr',0);
UpdateTimeIndex(hfig);
DrawTimeSeries(hfig,cIX,gIX);

%%


% Normalized Leaky Kernel Affine Projection Algorithm
%
% W. Liu and J.C. Principe, "Kernel Affine Projection Algorithms", EURASIP
% Journal on Advances in Signal Processing, Volume 2008, Article ID 784292,
% 12 pages. http://dx.doi.org/10.1155/2008/784292
%
% Remark: This implementation includes a maximum dictionary size M. With
% M=Inf this algorithm is equivalent to KAPA-4 from the publication. With
% M=Inf and lambda=0 it is equivalent to KAPA-2.
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

classdef nlkapa < kernel_adaptive_filter

    properties (GetAccess = 'public', SetAccess = 'private') % parameters
        eta = .05; % learning rate
        eps = 1E-4; % Newton regularization
        lambda = 1E-2; % Tikhonov regularization
        M = 1000; % maximum dictionary size
        P = 20; % number of regressors
        kerneltype = 'gauss'; % kernel type
        kernelpar = 1; % kernel parameter
    end

    properties (GetAccess = 'protected', SetAccess = 'private') % variables
        xmem = []; % input memory
        ymem = []; % output memory
        dict = []; % dictionary
        alpha = []; % expansion coefficients
    end

    methods
        function kaf = nlkapa(parameters) % constructor
            if (nargin > 0) % copy valid parameters
                for fn = fieldnames(parameters)'
                    if ismember(fn,fieldnames(kaf))
                        kaf.(fn{1}) = parameters.(fn{1});
                    end
                end
            end
        end

        function y_est = evaluate(kaf,x) % evaluate the algorithm
            if size(kaf.dict,1)>0
                k = kernel(kaf.dict,x,kaf.kerneltype,kaf.kernelpar);
                y_est = k'*kaf.alpha;
            else
                y_est = zeros(size(x,1),1);
            end
        end

        function train(kaf,x,y) % train the algorithm
            if size(kaf.dict,2)==0 % initialize
                kaf.dict = x;
                kaf.alpha = kaf.eta*y;
                kaf.xmem = x;
                kaf.ymem = y;
            else
                if size(kaf.dict,1) < kaf.M
                    if size(kaf.xmem,1)<kaf.P
                        % grow memory
                        kaf.xmem = [kaf.xmem; x];
                        kaf.ymem = [kaf.ymem; y];
                    else
                        % slide memory
                        kaf.xmem = [kaf.xmem(2:kaf.P,:); x];
                        kaf.ymem = [kaf.ymem(2:kaf.P); y];
                    end

                    ymem_est = kaf.evaluate(kaf.xmem);
                    e = kaf.ymem - ymem_est;
                    G = kernel(kaf.xmem,kaf.xmem,...
                        kaf.kerneltype,kaf.kernelpar);

                    kaf.dict = [kaf.dict; x]; % grow dictionary
                    kaf.alpha = [(1-kaf.lambda*kaf.eta)*kaf.alpha; 0];
                    % leak and grow coefficients

                    m = size(kaf.alpha,1);
                    p = size(kaf.xmem,1);

                    % update p last coefficients
                    kaf.alpha(m-p+1:m) = kaf.alpha(m-p+1:m) + ...
                        kaf.eta*inv(G+kaf.eps*eye(p))*e; %#ok<MINV>
                    % prefer inv to \ to avoid instability
                end
            end
        end
    end
end


function V = mttkrp(X,U,n)
%MTTKRP Matricized tensor times Khatri-Rao product for ktensor.
%
%   V = MTTKRP(X,U,n) efficiently calculates the matrix product of the
%   n-mode matricization of X with the Khatri-Rao product of all
%   entries in U, a cell array of matrices, except the nth.  How to
%   most efficiently do this computation depends on the type of tensor
%   involved.
%
%   See also KTENSOR, KTENSOR/TTV
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

N = ndims(X);

if (n==1)
    R = size(U{2},2);
else
    R = size(U{1},2);
end

% Compute matrix of weights
W = repmat(X.lambda,1,R);
for i = [1:n-1,n+1:N]
  W = W .* (X.u{i}' * U{i});
end

% Find each column of answer by multiplying columns of X.u{n} with weights
V = X.u{n} * W;


function jac = p03_jac ( neqn, t, y )

%*****************************************************************************80
%
%% P03_JAC evaluates the jacobian for problem p03.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the jacobian.
%
%    Output, real JAC(NEQN,NEQN), the jacobian matrix.
%
  jac(1,1) = cos ( t );

  return
end


function asa241_test01 ( )

%*****************************************************************************80
%
%% TEST01 tests R4_NORMAL_01_CDF_INVERSE, NORMAL_01_CDF_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 November 2006
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01:\n' );
  fprintf ( 1, '  Let FX = NormalCDF ( X ).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  NORMAL_01_CDF_VALUES returns some values of ( X, FX ).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  R4_NORMAL_01_CDF_INVERSE takes FX and computes\n' );
  fprintf ( 1, '    an estimate X2, of the corresponding input argument,\n' );
  fprintf ( 1, '    accurate to about 7 decimal places.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X                       FX               X2\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = normal_01_cdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    x2 = r4_normal_01_cdf_inverse ( fx );

    fprintf ( 1, '  %22.16e  %22.16e  %22.16e\n', x, fx, x2 );

  end

  return
end


function [ xyz, line_pointer, line_data ] = xyzl_example ( point_num, ...
  line_num, line_data_num )

%*****************************************************************************80
%
%% XYZL_EXAMPLE sets data suitable for a pair of XYZ and XYZL files.
%
%  Discussion:
%
%    There are 8 points.
%    There are 6 lines.
%    There are 18 line items.
%
%       8------7
%      /|     /|
%     / |    / |
%    5------6  |
%    |  4---|--3
%    | /    | /
%    |/     |/
%    1------2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, integer LINE_NUM, the number of lines.
%
%    Input, integer  LINE_DATA_NUM, the number of line items.
%
%    Output, real XY(2,POINT_NUM), the point coordinates.
%
%    Output, integer LINE_POINTER(LINE_NUM+1), pointers to the
%    first line item for each line.
%
%    Output, integer LINE_DATA(LINE_DATA_NUM), indices
%    of points that form lines.
%
  xyz(1:3,1:point_num) = [ ...
     0.0,  0.0,  0.0; ...
     1.0,  0.0,  0.0; ...
     1.0,  1.0,  0.0; ...
     0.0,  1.0,  0.0; ...
     0.0,  0.0,  1.0; ...
     1.0,  0.0,  1.0; ...
     1.0,  1.0,  1.0; ...
     0.0,  1.0,  1.0 ]';

  line_pointer(1:line_num+1) = [ 1, 6, 11, 13, 15, 17, 19 ];

  line_data(1:line_data_num) = [ ...
     1,  2,  3,  4,  1, ...
     5,  6,  7,  8,  5, ...
     1,  5, ...
     2,  6, ...
     3,  7, ...
     4,  8 ];

  return
end


function B = rot3d90(A, X, K)
% ROT3D90 - rotate 3D array 90 degrees around a specific axis
%
%   B = rot3D90(A, X) is the 90 degree counterclockwise rotation of the 3D
%   array A along a specific axis. X denotes the plane of rotation,
%   according to the table below. For instance, when X is 1 the plane of
%   rotation is formed by the 2nd and 3rd dimension.
%
%   rot3d90(A, X, K) is the K*90 degree rotation of A, K = +-1,+-2,...
%
%   If A has a dimension of [N, M, L], the dimensions of the rotation
%   depend on X. For value of X, the subindex in the X-th dimension will be
%   unaffected. Therefore, specific vectors will be unaffected.
%   Note that rot3d90(A,3) and rot90(A) produce the same result.
%
%      X    Rotation plane    Dimension of B   Unaffected vectors
%     - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%      1    dims 2 & 3        [N, L, M]        N-by-1 (row)
%      2    dims 1 & 3        [L, M, N]        1-by-N (column)
%      3    dims 1 & 2        [M, N, L]        1-by-1-by-N (plane)
%
%   Example:
%      A = cat(3,[1 2 ; 3 4],[5 6 ; 7 8])
%      B = rot3d90(A, 2) % rotation in plane formed by 1st and 3rd dimension
%      % B(:,:,1) = 5  6
%      %            1  2
%      % B(:,:,2) = 7  8
%      %            3  4
%
%   See also ROT90, FLIP

% version 3.1, jan 2018
% (c) Jos van der Geest
% email: samelinoa@gmail.com
% FEX: http://www.mathworks.nl/matlabcentral/fileexchange/authors/10584

% 1.0 (jan 2018) created, using rot90 and permute
% 2.0 (jan 2018) replaced calls to rot90 by direct permuting and flipping
% 3.0 (jan 2018) added K, specifiying the number of rotations
% 3.1 (jan 2018) fixed an error for 2D arrays inputs

narginchk(2, 3)

if nargin == 2
    K = 1 ;
else
    if ~isscalar(K) || fix(K) ~= K
        error('rot3d90:KInvalid', 'K should be a scalar integer.');
    end
    K = mod(K, 4) ; % after 4 rotations we are back to A
end

B = A ;
for j = 1:K
    switch X
        case 1
            % rotation in the plane formed by the 2nd and 3rd dimension
            % N-by-1-by-1 (column) vectors stay the same
            % for all elements, the subindex in the 1st dim does not change
            B = permute(B, [1 3 2 4:ndims(B)]);
            B = flip(B,2);
        case 2
            % rotation in the plane formed by 1st and 3rd dimension
            % 1-by-N-by-1 (row) vectors stay the same
            % for all elements, the subindex in the 2nd dim does not change
            B = permute(B, [2 3 1 4:ndims(B)]) ;
            B = flip(B, 2);
            B = permute(B, [2 1 3:ndims(B)]);
        case 3
            % rotate in the plane formed by 1st and 2nd dimension
            % 1-by-1-by-N vectors stay the same
            % for all elements, the subindex in the 3rd dim does not change
            B = flip(B, 2);
            B = permute(B, [2 1 3:ndims(B)]);
        otherwise
            error('rot3d90:XInvalid', 'Invalid rotation axis. X should be 1, 2, or 3.') ;
    end
end


% -----------------------------------------------------------------  %
% Matlab Programs included the Appendix B in the book:               %
%  Xin-She Yang, Engineering Optimization: An Introduction           %
%                with Metaheuristic Applications                     %
%  Published by John Wiley & Sons, USA, July 2010                    %
%  ISBN: 978-0-470-58246-6,   Hardcover, 347 pages                   %
% -----------------------------------------------------------------  %
% Citation detail:                                                   %
% X.-S. Yang, Engineering Optimization: An Introduction with         %
% Metaheuristic Application, Wiley, USA, (2010).                     %
%                                                                    %
% http://www.wiley.com/WileyCDA/WileyTitle/productCd-0470582464.html %
% http://eu.wiley.com/WileyCDA/WileyTitle/productCd-0470582464.html  %
% -----------------------------------------------------------------  %
% ===== ftp://  ===== ftp://   ===== ftp:// =======================  %
% Matlab files ftp site at Wiley                                     %
% ftp://ftp.wiley.com/public/sci_tech_med/engineering_optimization   %
% ----------------------------------------------------------------   %

% Simulated Annealing for constrained optimization
% by Xin-She Yang @ Cambridge University @2008
% Usage: sa_mincon(alpha)

function [bestsol,fmin,N]=sa_mincon(alpha)

% Default cooling factor
if nargin<1,
    alpha=0.95;
end

% Display usage
disp('sa_mincon or [Best,fmin,N]=sa_mincon(0.95)');

% d dimensions

% Welded beam design optimization
Lb=[0.1 0.1  0.1 0.1];
Ub=[2.0 10.0 10.0 2.0];
u0=(Lb+Ub)/2;

if length(Lb) ~=length(Ub),
    disp('Simple bounds/limits are improper!');
    return
end

%% Start of the main program -----------------------------------------
d=length(Lb);       % Dimension of the problem

% Initializing parameters and settings
T_init = 1.0;       % Initial temperature
T_min =  1e-10;     % Finial stopping temperature
F_min = -1e+100;    % Min value of the function
max_rej=500;        % Maximum number of rejections
max_run=150;        % Maximum number of runs
max_accept = 50;    % Maximum number of accept
initial_search=500; % Initial search period
k = 1;              % Boltzmann constant
Enorm=1e-5;         % Energy norm (eg, Enorm=1e-8)

% Initializing the counters i,j etc
i= 0; j = 0; accept = 0; totaleval = 0;
% Initializing various values
T = T_init;
E_init = Fun(u0);
E_old = E_init; E_new=E_old;
best=u0;  % initially guessed values
% Starting the simulated annealling
while ((T > T_min) & (j <= max_rej) & E_new>F_min)
    i = i+1;
    % Check if max numbers of run/accept are met
    if (i >= max_run) | (accept >= max_accept)
        % reset the counters
        i = 1;  accept = 1;
      % Cooling according to a cooling schedule
        T = cooling(alpha,T);
    end

    % Function evaluations at new locations
    if totaleval<initial_search,
        init_flag=1;
        ns=newsolution(u0,Lb,Ub,init_flag);
    else
        init_flag=0;
        ns=newsolution(best,Lb,Ub,init_flag);
    end

      totaleval=totaleval+1;
      E_new = Fun(ns);
    % Decide to accept the new solution
    DeltaE=E_new-E_old;
    % Accept if improved
    if (DeltaE <0)
        best = ns; E_old = E_new;
        accept=accept+1;   j = 0;
    end
    % Accept with a small probability if not improved
    if (DeltaE>=0 & exp(-DeltaE/(k*T))>rand );
        best = ns; E_old = E_new;
        accept=accept+1;
    else
        j=j+1;
    end
    % Update the estimated optimal solution
    f_opt=E_old;
end

bestsol=best
fmin=f_opt
N=totaleval

%% New solutions
function s=newsolution(u0,Lb,Ub,init_flag)
  % Either search around
if length(Lb)>0 & init_flag==1,
  s=Lb+(Ub-Lb).*rand(size(u0));
else
  % Or local search by random walk
  s=u0+0.01*(Ub-Lb).*randn(size(u0));
end

s=bounds(s,Lb,Ub);

%% Cooling
function T=cooling(alpha,T)
T=alpha*T;

function ns=bounds(ns,Lb,Ub)
if length(Lb)>0,
% Apply the lower bound
  ns_tmp=ns;
  I=ns_tmp<Lb;
  ns_tmp(I)=Lb(I);
  % Apply the upper bounds
  J=ns_tmp>Ub;
  ns_tmp(J)=Ub(J);
% Update this new move
  ns=ns_tmp;
else
  ns=ns;
end

% d-dimensional objective function
function z=Fun(u)

% Objective
z=fobj(u);

% Apply nonlinear constraints by penalty method
% Z=f+sum_k=1^N lam_k g_k^2 *H(g_k)
z=z+getnonlinear(u);

function Z=getnonlinear(u)
Z=0;
% Penalty constatn
lam=10^15; lameq=10^15;
[g,geq]=constraints(u);

% Inequality constraints
for k=1:length(g),
    Z=Z+ lam*g(k)^2*getH(g(k));
end

% Equality constraints (when geq=[], length->0)
for k=1:length(geq),
   Z=Z+lameq*geq(k)^2*geteqH(geq(k));
end

% Test if inequalities hold
function H=getH(g)
if g<=0,
    H=0;
else
    H=1;
end

% Test if equalities hold
function H=geteqH(g)
if g==0,
    H=0;
else
    H=1;
end

% Objective functions
function z=fobj(u)
% Welded Beam Design Optimization
% K. Ragsdell and D. Phillips, Optimal design of a class of welded
% strucures using geometric programming,
% J. Eng. Ind., 98 (3):1021-1025, (1976).
% Best solution found in literature
% [0.205730, 3.470489, 9.036624, 0.205729) with the objective 1.724852
% by Cagnina et al., Solving engineering optimization problems with the
% simple constrained particle swarm optimizer, Informatica, 32 (2008)319-326

z=1.10471*u(1)^2*u(2)+0.04811*u(3)*u(4)*(14.0+u(2));

% For Rosenbrock's function
% z=(1-u(1))^2+(u(2)-u(1)^2)^2+(1-u(3))^2+(1-u(4))^2

% All constraints
function [g,geq]=constraints(x)
% Inequality constraints
Q=6000*(14+x(2)/2);
D=sqrt(x(2)^2/4+(x(1)+x(3))^2/4);
J=2*(x(1)*x(2)*sqrt(2)*(x(2)^2/12+(x(1)+x(3))^2/4));
alpha=6000/(sqrt(2)*x(1)*x(2));
beta=Q*D/J;
tau=sqrt(alpha^2+2*alpha*beta*x(2)/(2*D)+beta^2);
sigma=504000/(x(4)*x(3)^2);
delta=65856000/(30*10^6*x(4)*x(3)^3);
F=4.013*(30*10^6)/196*sqrt(x(3)^2*x(4)^6/36)*(1-x(3)*sqrt(30/48)/28);

g(1)=tau-13600;
g(2)=sigma-30000;
g(3)=x(1)-x(4);
g(4)=0.10471*x(1)^2+0.04811*x(3)*x(4)*(14+x(2))-5.0;
g(5)=0.125-x(1);
g(6)=delta-0.25;
g(7)=6000-F;

% Equality constraints
geq=[];
%% End of the program ------------------------------------------------


function [xMin,fMin,exitCode]=steepestDescent(f,x0,epsilon,deltaTestDist,delta,lineSearchParams,maxIter)
%%STEEPESTDESCENT Perform unconstrained nonlinear optimization using
%              the steepest descent algorithm. The algorithm performs
%              unconstrained minimization of a nonlinear function without
%              having to provide a Hessian matrix.
%
%INPUTS: f A handle to the function (and its gradient) over which the
%          minimization is to be performed. The function [fVal,gVal]=f(x)
%          takes the NX1 x vector and returns the real scalar function
%          value fVal and gradient gVal at the point x.
%       x0 The NX1-dimensional point from which the minimization starts.
%  epsilon The parameter determining the accuracy of the desired solution
%          in terms of the gradient. The function terminates when
%          norm(g) < epsilon*max([1, norm(x)])
%          where g is the gradient. The default if omitted or an empty
%          matrix is passed is 1e-6.
% deltaTestDist The number of iterations back to use to compute the
%          decrease of the objective function if a delta-based convergence
%          test is performed. If zero, then no delta-based convergence
%          testing is done. The default if omitted or an empty matrix is
%          passed is zero.
%    delta The delta for the delta convergence test. This determines the
%          minimum rate of decrease of the objective function. Convergence
%          is determined if (f'-f)<=delta*f, where f' is the value of the
%          objective function f deltaTestDist iterations ago,and f is the
%          current objective function value. The default if this parameter
%          is omitted or an empty matrix is passed is 0.
% lineSearchParams An optional structure whose members specify tolerances
%          for the line search. The parameters are described as in the
%          lineSearch function, except if -1 is passed instead of a
%          parameter structure, then no line search is performed.
%  maxIter The maximum number of iterations to use for the algorithm. The
%          default if this parameter is omitted or an empty matrix is
%          passed is 100.
%
%OUTPUTS: xMin The value of x at the minimum point found. If exitCode is
%              negative, then this might be an empty matrix.
%         fMin The cost function value at the minimum point found. If
%              exitCode is negative, then this might be an empty matrix.
%     exitCode A value indicating the termination condition of the
%              algorithm. Nonnegative values indicate success; negative
%              values indicate some type of failure. Possible values are:
%                  0 The algorithm termiated successfully based on the
%                    gradient criterion.
%                  1 The algorithm terminated successfully based on the
%                    accuracy criterion.
%                 -1 The maximum number of overall iterations was reached.
%                 -2 A non-finite value was encoutered outside the line
%                    search.
%              Other negative values correspond to a failure in lineSearch
%              and correspond to the exitCode returned by the lineSearch
%              function.
%
%The algorithm is implemented based on the description of steepest descent
%given in Chapter 1.2 of [1].
%
%EXAMPLE:
%The example is that used in the lineSearch file.
% f=@(x)deal((x(1)+x(2)-3)*(x(1)+x(2))^3*(x(1)+x(2)-6)^4,... %The function
%            [(-6+x(1)+x(2))^3*(x(1)+x(2))^2*(54+8*x(1)^2+x(2)*(-45+8*x(2))+x(1)*(-45+16*x(2)));
%            (-6+x(1)+x(2))^3*(x(1)+x(2))^2*(54+8*x(1)^2+x(2)*(-45+8*x(2))+x(1)*(-45+16*x(2)))]);%And the gradient as the second return.
% %Note that the deal function is used to make an anonymous function have
% %two outputs.
% x0=[0.5;0.25];
% [xMin,fMin,exitCode]=steepestDescent(f,x0)
%The optimum point found is such that sum(xMin) is approximately
%1.73539450 with a minimum function value of approximately -2.1860756.
%
%REFERENCES:
%[1] D. P. Bertsekas, Nonlinear Programming, 3rd ed. Belmont, MA: Athena
%    Science, 2016.
%
%January 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release

    if(nargin<3||isempty(epsilon))
        epsilon=1e-6;
    end

    if(nargin<4||isempty(deltaTestDist))
        deltaTestDist=0;
    end

    if(nargin<5||isempty(delta))
        delta=0;
    end

    %The default line search parameter options
    useLineSearch=true;
    if(nargin>=6&&~isempty(lineSearchParams))
        if(isnumeric(lineSearchParams)&&lineSearchParams==-1)
            %If one just wishes to blindly take steps.
            useLineSearch=false;
        elseif(~isstruct(lineSearchParams)||(isnumeric(lineSearchParams)&&lineSearchParams~=-1))
            error('Unknown lineSearchParams value given')
        end
    else
        lineSearchParams=[];
    end

    if(nargin<7||isempty(maxIter))
        maxIter=100;
    end

    x=x0;
    [fValCur,gradF]=f(x0);

    if(deltaTestDist>0)
        pastFVals=zeros(deltaTestDist,1);
        pastFVals(1)=fValCur;
    end

    for cutIter=1:maxIter
         %Steepest descent direction.
         d=-gradF;

        if(useLineSearch)
            %Perform a line search in the given descent direction.
            [xNew,fValNew,gradFCur,~,~,exitCode]=lineSearch(f,x,d,[],[],lineSearchParams);
            if(isempty(xNew))
                %Just return the last valid values.
                xMin=x;
                fMin=fValCur;
                return;
            end
            xCur=xNew;
            fValCur=fValNew;
        else%Take a step without a line search.
            xCur=x+d;
            [fValCur,gradFCur]=f(xCur);
        end

        if(any(~isfinite(xCur(:))))
            exitCode=-2;
            xMin=[];
            fMin=[];
            return;
        end

        %Check for convergence based on the gradient.
        if(norm(gradFCur)<epsilon*max([1,norm(xCur)]))
            xMin=xCur;
            fMin=fValCur;
            exitCode=0;
            return;
        end

        %Check for convergence based on the actual function value.
        if(deltaTestDist~=0)
            if(pastFVals(end)-fValCur<=delta*fValCur)
                xMin=xCur;
                fMin=fValCur;
                exitCode=1;
                return;
            end
            pastFVals=circshift(pastFVals,[1,0]);
            pastFVals(1)=fValCur;
        end

        x=xCur;
        gradF=gradFCur;
    end

    %The maximum number of iterations elapsed without convergence
    xMin=xCur;
    fMin=fValCur;
    exitCode=-1;
end

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


% ----------------------------------------------------------------------- %
% Function plot_areaerrorbar plots the mean and standard deviation of a   %
% set of data filling the space between the positive and negative mean    %
% error using a semi-transparent background, completely customizable.     %
%                                                                         %
%   Input parameters:                                                     %
%       - data:     Data matrix, with rows corresponding to observations  %
%                   and columns to samples.                               %
%       - options:  (Optional) Struct that contains the customized params.%
%           * options.handle:       Figure handle to plot the result.     %
%           * options.color_area:   RGB color of the filled area.         %
%           * options.color_line:   RGB color of the mean line.           %
%           * options.alpha:        Alpha value for transparency.         %
%           * options.line_width:   Mean line width.                      %
%           * options.x_axis:       X time vector.                        %
%           * options.error:        Type of error to plot (+/-).          %
%                   if 'std',       one standard deviation;               %
%                   if 'sem',       standard error mean;                  %
%                   if 'var',       one variance;                         %
%                   if 'c95',       95% confidence interval.              %
% ----------------------------------------------------------------------- %
%   Example of use:                                                       %
%       data = repmat(sin(1:0.01:2*pi),100,1);                            %
%       data = data + randn(size(data));                                  %
%       plot_areaerrorbar(data);                                          %
% ----------------------------------------------------------------------- %
%   Author:  Victor Martinez-Cagigal                                      %
%   Date:    30/04/2018                                                   %
%   E-mail:  vicmarcag (at) gmail (dot) com                               %
% ----------------------------------------------------------------------- %
function plot_areaerrorbar(data, options)
    % Default options
    if(nargin<2)
        options.handle     = figure(1);
        options.color_area = [128 193 219]./255;    % Blue theme
        options.color_line = [ 52 148 186]./255;
        %options.color_area = [243 169 114]./255;    % Orange theme
        %options.color_line = [236 112  22]./255;
        options.alpha      = 0.5;
        options.line_width = 2;
        options.error      = 'std';
    end
    if(isfield(options,'x_axis')==0), options.x_axis = 1:size(data,2); end
    options.x_axis = options.x_axis(:);

    % Computing the mean and standard deviation of the data matrix
    data_mean = mean(data,1);
    data_std  = std(data,0,1);

    % Type of error plot
    switch(options.error)
        case 'std', error = data_std;
        case 'sem', error = (data_std./sqrt(size(data,1)));
        case 'var', error = (data_std.^2);
        case 'c95', error = (data_std./sqrt(size(data,1))).*1.96;
    end

    % Plotting the result
    figure(options.handle);
    x_vector = [options.x_axis', fliplr(options.x_axis')];
    patch = fill(x_vector, [data_mean+error,fliplr(data_mean-error)], options.color_area);
    set(patch, 'edgecolor', 'none');
    set(patch, 'FaceAlpha', options.alpha);
    hold on;
    plot(options.x_axis, data_mean, 'color', options.color_line, ...
        'LineWidth', options.line_width);
    hold off;

end


% Convert adjacency matrix (nxn) to edge list (mx3)
%
% INPUTS: adjacency matrix: nxn
% OUTPUTS: edge list: mx3
%
% GB: last updated, Sep 24, 2012

function el=adj2edgeL(adj)

n=length(adj); % number of nodes
edges=find(adj>0); % indices of all edges

el=[];
for e=1:length(edges)
  [i,j]=ind2sub([n,n],edges(e)); % node indices of edge e
  el=[el; i j adj(i,j)];
end


function mean = lorentz_mean ( )

%*****************************************************************************80
%
%% LORENTZ_MEAN returns the mean of the Lorentz PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 1999
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real MEAN, the mean of the PDF.
%
  mean = 0.0;

  return
end


function geometry_test020 ( )

%*****************************************************************************80
%
%% TEST020 tests CUBE_SIZE_3D, CUBE_SHAPE_3D, SHAPE_PRINT_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST020\n' );
  fprintf ( 1, '  For the cube,\n' );
  fprintf ( 1, '  CUBE_SIZE_3D returns dimension information;\n' );
  fprintf ( 1, '  CUBE_SHAPE_3D returns face and order information.\n' );
  fprintf ( 1, '  SHAPE_PRINT_3D prints this information.\n' );

  [ point_num, edge_num, face_num, face_order_max ] = cube_size_3d ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of points =   %d\n', point_num );
  fprintf ( 1, '  Number of edges =    %d\n', edge_num );
  fprintf ( 1, '  Number of faces =    %d\n', face_num );
  fprintf ( 1, '  Maximum face order = %d\n', face_order_max );

  [ point_coord, face_order, face_point ] = cube_shape_3d ( ...
    point_num, face_num, face_order_max );

  shape_print_3d ( point_num, face_num, face_order_max, ...
    point_coord, face_order, face_point );

  return
end


%INCLINING21	Inclining Experiment data for Example 7.1.
% Data taken from Hansen (1985), `An analytical treatment of the accuracy of the
% results of the inclining experiment', Naval Engineers Journal, Vol. 97, No.4,
% May, pp. 97-115.
% Format is [ moment tangent ], initial units [ ft-tons - ]
% Companion file for Biran, A. (2003), Ship Hydrostatics and Stability,
% Oxford: Butterworth-Heinemann.

incldata = [
 3853.7	 0.01869
 3853.7	 0.01854
 3853.7	 0.01788
 2570.1	 0.01259
 2570.1  0.01255
 2570.1	 0.01214
 1286.8	 0.00648
 1286.8	 0.00641
 1286.8	 0.00615
    3.8	 0.00042
    3.8	 0.00047
    3.8	 0.00000
-3785.8	-0.01791
-3785.8 -0.01802
-3785.8 -0.01850
-2523.3 -0.01194
-2523.3 -0.01203
-2523.3 -0.01235
-1263.7 -0.00574
-1263.7 -0.00599
-1263.7 -0.00646
   -0.6 -0.00000
   -0.6 -0.00000
   -0.6 -0.00063 ];
format bank
% separate data and convert to SI units
moment  = (0.305/1.016)*incldata(:, 1);
tangent = incldata(:, 2);
plot(tangent, moment, 'k.'), grid
Hl = ylabel('Inclining moment, pd, tm');
set(Hl, 'FontSize', 14)
Hl = xlabel('Heel angle tangent, tan\theta');
set(Hl, 'FontSize', 14)

hold on
tmin = min(tangent);
tmax = max(tangent);
M    = sum(tangent.*moment)/sum(tangent.^2);
Mmin = M*tmin;
Mmax = M*tmax;
plot( [ tmin tmax ], [ Mmin Mmax ], 'k-')
Ht = text(-0.015, 1100, [ 'Average slope = ' num2str(M) ]);
set(Ht, 'FontSize', 14)

hold off


function boolVal=pointIsInTriangle(point2D,triVertices)
%%POINTISINTRIANGLE Determine whether a specified 2D points is within a
%           triangle.
%
%INPUTS: point2D A 2XnumPts set of 2D points.
%    triVertices A 2X3 set of vertices of the triangle in any order.
%
%OUTPUTS: bookVal A 1XnumPts set of boolean values that are true if the
%                 point is in the triangle or is on an edge of the
%                 triangle.
%
%The function pt2TriangCoords2D is used to convert the points to 2D
%triangular barycentric coordinates. If the point is within the triangle,
%then all of the cooridnates will be positive. Otherwise, some coordinates
%will be negative.
%
%EXAMPLE:
%In this example, the first 3 points are in or on the triangle and the
%final two points are outside the trangle. Thus, boolPoints=[1,1,1,0,0];
% v=[0,1,0.5;
%    0,0,0.5];
% points=[0.25,0,1,10,2;
%         0.25,0,0,-1,0];
% boolVals=pointIsInTriangle(points,v)
%
%September 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    numPts=size(point2D,2);
    phi=pt2TriangCoords2D(point2D,triVertices);

    boolVal=false(1,numPts);
    for k=1:numPts
        boolVal(k)=all(phi(:,k)>=0);
    end
end

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


function wfiltdtinfo(dw,varargin)
%WFILTDTINFO Plots dual-tree filters info
%   Usage: wfiltdtinfo(dw);
%
%   Input parameters:
%         dw     : Wavelet dual-tree filterbank
%
%   `wfiltdtinfo(w)` plots impulse responses, frequency responses and
%   approximation of the scaling and of the wavelet function(s) associated
%   with the dual-tree wavelet filters defined by *w* in a single figure.
%   Format of *dw* is the same as in |dtwfb|.
%
%   The figure is organized as follows:
%
%   First row shows impulse responses of the first (real) tree.
%
%   Second row shows impulse responses of the second (imag) tree.
%
%   Third row contains plots of real (green), imaginary (red) and absolute
%   (blue) parts of approximation of scaling and wavelet function(s).
%
%   Fourth and fifth row show magnitude and phase frequency responses
%   respectivelly of filters from rows 1 and 2 with matching colors.
%
%   Optionally it is possible to define scaling of the y axis of the
%   frequency seponses. Supported are:
%
%   'db','lin'
%       dB or linear scale respectivelly. By deault a dB scale is used.
%
%   Examples:
%   ---------
%   :::
%      wfiltdtinfo('qshift4');
%

% AUTHOR: Zdenek Prusa

complainif_notenoughargs(nargin,1,'WFILTDTINFO');

definput.flags.freqzscale = {'db','lin'};
[flags]=ltfatarghelper({},definput,varargin);

[dwstruct,info] = dtwfbinit({'strict',{dw,6,'dwt'}});
dw = info.dw;

filtNo = size(dw.g,1);
grayLevel = [0.6,0.6,0.6];
clf;

colorAr ={repmat('rbk',1,filtNo),repmat('cmg',1,filtNo)};

for ii=1:2
    subplot(5,filtNo,filtNo*(ii-1)+1);
    title(sprintf('Scaling imp. response, tree %i',ii));
    loAna = dw.g{1,ii}.h;
    loShift = -dw.g{1,ii}.offset;
    xvals = -loShift + (0:length(loAna)-1);
    hold on;
    if ~isempty(loAna(loAna==0))
       stem(xvals(loAna==0),loAna(loAna==0),'Color',grayLevel);
    end

    loAnaNZ = find(loAna~=0);
    stem(xvals(loAnaNZ),loAna(loAnaNZ),colorAr{ii}(1));
    axis tight;
    hold off;
end

for ii=1:2
    for ff=2:filtNo
        subplot(5,filtNo,ff+filtNo*(ii-1));
        title(sprintf('Wavelet imp. response no: %i, tree %i',ff-1,ii));
        filtAna = dw.g{ff,ii}.h;
        filtShift = -dw.g{ff,ii}.offset;
        xvals = -filtShift + (0:length(filtAna)-1);
        filtNZ = find(filtAna~=0);
        hold on;

        if ~isempty(filtAna(filtAna==0))
           stem(xvals(filtAna==0),filtAna(filtAna==0),'Color',grayLevel);
        end

        stem(xvals(filtNZ),filtAna(filtNZ),colorAr{ii}(ff));
        axis tight;
        hold off;
    end
end

L =  wfbtlength(1024,dwstruct,'per');
Lc = wfbtclength(L,dwstruct,'per');
c = wavpack2cell(zeros(sum([Lc;Lc(end:-1:1)]),1),...
                 [Lc;Lc(end:-1:1)]);
c{1}(1) = 1;
sfn = idtwfb(c,dwstruct,L);

subplot(5,filtNo,[2*filtNo+1]);
xvals = ((-floor(L/2)+1:floor(L/2)).');
plot(xvals,fftshift([abs(sfn),real(sfn),imag(sfn)],1));
axis tight;
title('Scaling function');

%legend({'abs','real','imag'},'Location','south','Orientation','horizontal')

for ff=2:filtNo
   subplot(5,filtNo,[2*filtNo+ff]);

   c{ff-1}(1) = 0;
   c{ff}(1) = 1;
   wfn = idtwfb(c,dwstruct,L);

   plot(xvals,fftshift([abs(wfn),real(wfn),imag(wfn)],1));
   axis tight;
   %legend({'abs','real','imag'},'Location','south','Orientation','horizontal')
   title(sprintf('Wavelet function: %i',ff-1));
end

subplot(5,filtNo,3*filtNo + (1:filtNo) );
title('Magnitude frequency response');
maxLen=max(cellfun(@(gEl) numel(gEl.h),dw.g));
Ls = nextfastfft(max([maxLen,1024]));
H = filterbankfreqz(dw.g(:),[dw.a(:);dw.a(:)],Ls);

%[H] = wtfftfreqz(w.g);
if flags.do_db
   plotH = 20*log10(abs(H));
elseif flags.do_lin
   plotH = abs(H);
else
    error('%s: Unknown parameter',upper(mfilaname));
end
xVals = linspace(0,1,numel(H(:,1)));
hold on;
for ii=1:2
for ff=1:filtNo
   plot(xVals,plotH(:,ff+(ii-1)*filtNo),colorAr{ii}(ff));
   axis tight;
end
end
if flags.do_db
    ylim([-30,max(plotH(:))])
end
ylabel('|\itH|[dB]');
xlabel('\omega [-]')
hold off;

subplot(5,filtNo,4*filtNo + (1:filtNo) );
title('Phase frequency response');
hold on;
for ii=1:2
for ff=1:filtNo
   plot(xVals,unwrap(angle((H(:,ff+(ii-1)*filtNo))))/pi,colorAr{ii}(ff));
   axis tight;
end
end
ylabel('arg H(\omega)[\pi rad]');
xlabel('\omega [-]')

axis tight;
% plot(unwrap(angle([H])));
% axis tight;
hold off;


%[2010]-"Firefly algorithm,stochastic test functions and design
%optimization"

% (9/12/2020)

function FA = jFireflyAlgorithm(feat,label,opts)
% Parameters
lb    = 0;
ub    = 1;
thres = 0.5;
alpha = 1;      % constant
beta0 = 1;      % light amplitude
gamma = 1;      % absorbtion coefficient
theta = 0.97;   % control alpha

if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'beta0'), beta0 = opts.beta0; end
if isfield(opts,'gamma'), gamma = opts.gamma; end
if isfield(opts,'alpha'), alpha = opts.alpha; end
if isfield(opts,'theta'), theta = opts.theta; end
if isfield(opts,'thres'), thres = opts.thres; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = zeros(N,dim);
for i = 1:N
  for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
  end
end
% Fitness
fit  = zeros(1,N);
fitG = inf;
for i = 1:N
  fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  % Best solution
  if fit(i) < fitG
    fitG = fit(i);
    Xgb  = X(i,:);
  end
end
% Pre
curve = zeros(1,max_Iter);
curve(1) = fitG;
t = 2;
% Generation
while t <= max_Iter
  % Alpha update
  alpha      = alpha * theta;
  % Rank firefly based on their light intensity
  [fit, idx] = sort(fit,'ascend');
  X          = X(idx,:);
	for i = 1:N
    % The attractiveness parameter
    for j = 1:N
      % Update moves if firefly j brighter than firefly i
      if fit(i) > fit(j)
        % Compute Euclidean distance
        r    = sqrt(sum((X(i,:) - X(j,:)) .^ 2));
        % Beta (2)
        beta = beta0 * exp(-gamma * r ^ 2);
        for d = 1:dim
          % Update position (3)
          eps    = rand() - 0.5;
          X(i,d) = X(i,d) + beta * (X(j,d) - X(i,d)) + alpha * eps;
        end
        % Boundary
        XB = X(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
        X(i,:) = XB;
        % Fitness
        fit(i) = fun(feat,label,(X(i,:) > thres),opts);
        % Update global best firefly
        if fit(i) < fitG
          fitG = fit(i);
          Xgb  = X(i,:);
        end
      end
    end
  end
  curve(t) = fitG;
  fprintf('\nGeneration %d Best (FA)= %f',t,curve(t))
  t = t + 1;
end
% Select features
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
FA.sf = Sf;
FA.ff = sFeat;
FA.nf = length(Sf);
FA.c  = curve;
FA.f  = feat;
FA.l  = label;
end


function pde = quadCurlDataSmooth1
% (curl)^4 u = f on \Omega = [0,pi]^3
% u x n = 0
% (curl u) x n = 0
% div u = g
%
pde = struct('exactu', @exactu,'g_D', @g_D, 'g', @divu, ...
             'curlu', @curlu, 'curlcurlu',@curlcurlu, ...
             'tricurlu',@tricurlu,'quadcurlu',@quadcurlu, 'pentacurlu',@pentacurlu);

Scaling = 10; % scaling

    function s = exactu(p)
        x = p(:,1); y = p(:,2); z = p(:,3);
        s(:,1) = 0*x;
        s(:,2) = 0*y;
        s(:,3) = (sin(x)).^2.*(sin(y)).^2.*sin(z);
        s = s/Scaling;
    end

    function s = curlu(p)
        x = p(:,1); y = p(:,2); z = p(:,3);
        s(:,1) =  sin(2*y).*(sin(x)).^2.*sin(z);
        s(:,2) = -sin(2*x).*(sin(y)).^2.*sin(z);
        s(:,3) = 0*z;
        s = s/Scaling;
    end

    function s = curlcurlu(p)
        x = p(:,1); y = p(:,2); z = p(:,3);
        s(:,1) = sin(2*x).*(sin(y)).^2.*cos(z);
        s(:,2) = (sin(x)).^2.*sin(2*y).*cos(z);
        s(:,3) = 4*(sin(x)).^2.*(sin(y)).^2.*sin(z) ...
               - 2*(cos(y)).^2.*(sin(x)).^2.*sin(z) ...
               - 2*(cos(x)).^2.*(sin(y)).^2.*sin(z);
        s = s/Scaling;
    end

    function s = tricurlu(p)
        x = p(:,1); y = p(:,2); z = p(:,3);
        s(:,1) = 7*(sin(x)).^2.*sin(2*y).*sin(z) - 2*(cos(x)).^2.*sin(2*y).*sin(z);
        s(:,2) = 2*(cos(y)).^2.*sin(2*x).*sin(z) - 7*sin(2*x).*(sin(y)).^2.*sin(z);
        s(:,3) = 0*z;
        s = s/Scaling;
    end

    function s = quadcurlu(p)
        x = p(:,1); y = p(:,2); z = p(:,3);
        s(:,1) = 7*cos(z).*sin(2*x).*(sin(y)).^2 - 2*(cos(y)).^2.*cos(z).*sin(2*x);
        s(:,2) = 7*cos(z).*(sin(x)).^2.*sin(2*y) - 2*(cos(x)).^2.*cos(z).*sin(2*y);
        s(:,3) = 8*(cos(x)).^2.*(cos(y)).^2.*sin(z) - 18*(cos(x)).^2.*(sin(y)).^2.*sin(z) ...
              - 18*(cos(y)).^2.*(sin(x)).^2.*sin(z) + 28*(sin(x)).^2.*(sin(y)).^2.*sin(z);
        s = s/Scaling;
    end

    function s = pentacurlu(p)
        x = p(:,1); y = p(:,2); z = p(:,3);
        s(:,1) = 53*(sin(x)).^2.*sin(2*y).*sin(z) - 28*(cos(x)).^2.*sin(2*y).*sin(z);
        s(:,2) = 28*(cos(y)).^2.*sin(2*x).*sin(z) - 53*sin(2*x).*(sin(y)).^2.*sin(z);
        s(:,3) = 0*z;
        s = s/Scaling;
    end

    function s = g_D(p) % Dirichlet boundary condition for u
        s = exactu(p);
    end

    function s = divu(p)
        x = p(:,1); y = p(:,2); z = p(:,3);
        s = (sin(x)).^2.*(sin(y)).^2.*cos(z);
        s = s/Scaling;
    end
end


function c = crossProduct3d(a,b)
%CROSSPRODUCT3D Vector cross product faster than inbuilt MATLAB cross.
%
%   C = crossProduct3d(A, B)
%   returns the cross product of the two 3D vectors A and B, that is:
%       C = A x B
%   A and B must be N-by-3 element vectors. If either A or B is a 1-by-3
%   row vector, the result C will have the size of the other input and will
%   be the  concatenation of each row's cross product.
%
%   Example
%     v1 = [2 0 0];
%     v2 = [0 3 0];
%     crossProduct3d(v1, v2)
%     ans =
%         0   0   6
%
%
%   Class support for inputs A,B:
%      float: double, single
%
%   See also DOT.

% ------
% Author: Sven Holcombe
% e-mail: N/A

% HISTORY
% 2017-11-24 rename from vectorCross3d to crossProduct3d

% size of inputs
sizeA = size(a);
sizeB = size(b);

% Initialise c to the size of a or b, whichever has more dimensions. If
% they have the same dimensions, initialise to the larger of the two
switch sign(numel(sizeA) - numel(sizeB))
    case 1
        c = zeros(sizeA);
    case -1
        c = zeros(sizeB);
    otherwise
        c = zeros(max(sizeA, sizeB));
end

c(:) = bsxfun(@times, a(:,[2 3 1],:), b(:,[3 1 2],:)) - ...
       bsxfun(@times, b(:,[2 3 1],:), a(:,[3 1 2],:));


function [xout,yout]=rejectsample(f, n)
% [xout,yout]=rejectsample(f, n)
% rejection sampling from the 2D discrete distribution f
% n: number of samples
% the proposal distriburion is a uniform distribution on the range of f
sizef=size(f);
maxf=max(f(:));
nsofar=0;
while nsofar<n
    xrand=sizef(1)*rand;
    yrand=sizef(2)*rand;
    u=maxf*rand;
    if u<f(ceil(xrand), ceil(yrand))
        nsofar=nsofar+1;
        x(nsofar)=xrand;
        y(nsofar)=yrand;
    end
end

%to the middle of the pixels...
xout=x-0.5;
yout=y-0.5;


function J = jacobian( f, g )
%JACOBIAN   Jacobian determinant of two CHEBFUN2.
%   J = JACOBIAN(F,G) returns the Jacobian determinant of the Jacobian matrix.
%
%   Note we return the determinant of the Jacobian matrix and not the Jacobian
%   matrix itself.
%
% See also CHEBFUN2V/JACOBIAN.

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

% Empty check:
if ( isempty( f ) || isempty( g ) )
    J = [];
    return
end

% Call CHEBFUN2V/JACOBIAN():
J = jacobian( chebfun2v( { f, g } ) );

end


function square_domain
%square_domain   square domain Q2 grid generator
%   square_domain;
%
% grid defining data is saved to the file: square_grid.mat
%   IFISS function: DJS; 7 May 2006.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
fprintf('\n\nGrid generation for unit square  domain.\n')
nc=default('grid parameter: 3 for underlying 8x8 grid (default is 16x16)',4);
if nc<2, error('illegal parameter choice, try again.'), end
grid_type=default('uniform/stretched grid (1/2) (default is uniform)',1);
n=2^nc; np=n/2; nq=n/4;
%
%% compute (x,y) coordinates of vertices
% y-direction
if grid_type==2
   hmax=nc/(2^(nc+1));
   x1=-1;x2=-2*hmax;x3=2*hmax;x4=1;nx1=2^(nc-1)-1;nx2=2;nx3=2^(nc-1)-1;
   y1=-1;y2=-2*hmax;y3=2*hmax;y4=1;ny1=2^(nc-1)-1;ny2=2;ny3=2^(nc-1)-1;
   y=subint(y1,y2,y3,y4,ny1,ny2,ny3);
   stretch=(y(3)-y(2))/(y(2)-y(1));
   left=-1;
   x=y;
else
   square_type=default('[0,1] or [-1,1] square (enter 1/2) (default is [-1,1])',2);
   yy=[1/np:1/np:1];
   ypos=[0,yy];
   yneg=-yy(length(yy):-1:1);
   y=[yneg,ypos]';
   left=-1;
   if square_type==1
      y=[0:1/(2*np):1]'; left=0;
   end
   x=y;
   end
%
%% compute biquadratic element coordinates
nvtx=(n+1)*(n+1);
[X,Y]=meshgrid(x,y);
xx=reshape(X',nvtx,1);
yy=reshape(Y',nvtx,1);
xy=[xx(:),yy(:)];
%
kx = 1;
ky = 1;
mel=0;
for j=1:np
   for i=1:np
      mref=(n+1)*(ky-1)+kx;
      mel=mel+1;
      nvv(1) = mref;
      nvv(2) = mref+2;
      nvv(3) = mref+2*n+4;
      nvv(4) = mref+2*n+2;
      nvv(5) = mref+1;
      nvv(6) = mref+n+3;
      nvv(7) = mref+2*n+3;
      nvv(8)=  mref+n+1;
      nvv(9)=  mref+n+2;
      mv(mel,1:9)=nvv(1:9);
      kx = kx + 2;
   end
   ky = ky + 2;
   kx = 1;
end
%
%% compute boundary vertices and edges
% four boundary edges
k1=find( xy(:,2)==left );
e1=[]; for k=1:mel, if any(mv(k,5)==k1), e1=[e1,k]; end, end
ef1=ones(size(e1));
%
k2=find( xy(:,1)==1  & xy(:,2)<=1   & xy(:,2) >left);
e2=[]; for k=1:mel, if any(mv(k,6)==k2), e2=[e2,k]; end, end
ef2=2*ones(size(e2));
%
k3=find( xy(:,2)==1  & xy(:,1)<1   & xy(:,1) >left);
e3=[]; for k=1:mel, if any(mv(k,7)==k3), e3=[e3,k]; end, end
ef3=3*ones(size(e3));
%
k4=find( xy(:,1)==left & xy(:,2)<=1   & xy(:,2) >left );
e4=[]; for k=1:mel, if any(mv(k,8)==k4), e4=[e4,k]; end, end
ef4=4*ones(size(e4));
%
bound=sort([k1;k2;k3;k4]);
mbound=[e1',ef1';e2',ef2';e3',ef3';e4',ef4'];
%
%
outbc=1;
gohome
cd datafiles
save square_grid.mat mv xy bound mbound grid_type outbc x y
clear
return


function [V,F] = subdivided_sphere(iters,varargin)
  % SUBDIVIDED_SPHERE Generate a sphere by iteratively subdividing a
  % icosahedron in-plane and normalizing vertex locations to lie on the sphere.
  %
  % [V,F] = subdivided_sphere(iters,varargin)
  %
  % Inputs:
  %   iters  number of subdivision iterations (0 produces 12-vertex
  %     icosahedron)
  %   Optional:
  %     'Base'  followed by base shape.
  %     'Radius'  followed by scalar radius (multiplied against final V) {1}
  %     'SubdivisionMethod' followed by either:
  %       'loop'
  %       'sqrt3'
  %       {'upsample'}
  % Outputs:
  %   V  #V by 3 list of mesh vertices
  %   F  #F by 3 list of face indices into V
  %
  % See also: upsample, loop
  %

  % default values
  radius = 1;
  subdivision_method = 'upsample';
  base = '';
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Base','Radius','SubdivisionMethod'},{'base','radius','subdivision_method'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  switch base
  case 'icosahedron'
    [V,F] = icosahedron();
  case 'octahedron'
    [V,F] = octahedron();
  otherwise
    % Compute the 12 vertices
    phi = (1+sqrt(5))/2;  % Golden ratio
    V = [0   1  phi
            0  -1  phi
            0   1 -phi
            0  -1 -phi
            1  phi  0
          -1  phi  0
            1 -phi  0
          -1 -phi  0
            phi 0   1
          -phi 0   1
            phi 0  -1
          -phi 0  -1];
    % Scale to required radius
    V = V/(sqrt(1+phi^2));
    % Define the adjacency matrix
    F = [1  2  9
          1  9  5
          1  5  6
          1  6  10
          1  10 2
          2  7  9
          9  7  11
          9  11 5
          5  11 3
          5  3  6
          6  3  12
          6  12 10
          10 12 8
          10 8  2
          2  8  7
          4  7  8
          4  8  12
          4  12 3
          4  3  11
          4  11 7];
  end

  V = normalizerow(V);
  for iter = 1:iters
    switch subdivision_method
    case 'upsample'
      [V,F] = upsample(V,F);
    case 'loop'
      [V,F] = loop(V,F);
    case 'sqrt3'
      [V,F] = sqrt3(V,F);
    end
    V = normalizerow(V);

  end
  V = V*radius;

end


function [engine, loglik] = enter_evidence(engine, evidence, varargin)
% ENTER_EVIDENCE Add the specified evidence to the network (gaussian_inf_engine)
% [engine, loglik] = enter_evidence(engine, evidence, ...)
%
% evidence{i} = [] if if X(i) is hidden, and otherwise contains its observed value (scalar or column vector)

bnet = bnet_from_engine(engine);
ns = bnet.node_sizes;
O = find(~isemptycell(evidence));
H = find(isemptycell(evidence));
vals = cat(1, evidence{O});

% Compute Pr(H|o)
[Hmu, HSigma, loglik] = condition_gaussian(engine.mu, engine.Sigma, H, O, vals(:), ns);

engine.Hmu = Hmu;
engine.HSigma = HSigma;
engine.hnodes = H;

%%%%%%%%

function [mu2, Sigma2, loglik] = condition_gaussian(mu, Sigma, X, Y, y, ns)
% CONDITION_GAUSSIAN Compute Pr(X|Y=y) where X and Y are jointly Gaussian.
% [mu2, Sigma2, ll] = condition_gaussian(mu, Sigma, X, Y, y, ns)

if isempty(y)
  mu2 = mu;
  Sigma2 = Sigma;
  loglik = 0;
  return;
end

use_log = 1;

if length(Y)==length(mu) % instantiating every variable
  mu2 = y;
  Sigma2 = zeros(length(y));
  loglik = gaussian_prob(y, mu, Sigma, use_log);
  return;
end

[muX, muY, SXX, SXY, SYX, SYY] = partition_matrix_vec(mu, Sigma, X, Y, ns);
K = SXY*inv(SYY);
mu2 = muX + K*(y-muY);
Sigma2 = SXX - K*SYX;
loglik = gaussian_prob(y, muY, SYY, use_log);


function [pos] = surface_shift(pos, tri, amount)

% SURFACE_SHIFT inflates or deflates a triangulated surface by moving the
% vertices outward or inward along their normals.
%
% Use as
%   pos = surface_inflate(pos, tri, amount)
% where pos and tri describe the surface.
%
% See also SURFACE_NORMALS, SURFACE_ORIENTATION, SURFACE_INSIDE,
% SURFACE_NESTING

if isempty(tri)
  propos = elproj(pos);   % projection to 2D
  tri = delaunay(propos); % creates delaunay triangulation of 2D plane, which will be used for the the 3D case
end

nrm = surface_normals(pos, tri); % compute normals of surface
switch surface_orientation(pos, tri, nrm)
  case 'outward'
    % move along the normals
    nrm = +nrm;
  case 'inward'
    % move opposite to the normals
    nrm = -nrm;
  otherwise
    ft_warning('cannot determine the orientation of the surface');
end

% move the vertices with the specified amount along the normals
pos = pos + amount*nrm;


function seats=apport(votes,num,method,minseats)
%APPORT Political Apportionment
%   SEATS = APPORT(VOTES,NUM,METHOD) takes the array of VOTES for each
%   member and allocates seats according to METHOD until NUM seats have
%   been allocated.
%
%   APPORT(...,MINSEATS) pre-allocates MINSEATS to each member first,
%   guaranteeing that each member has at least MINSEATS number of SEATS.
%   MINSEATS can either be a vector, with length equal to the number of
%   members, or a scalar which is the same for all members. (Default is 0)
%
%   If either NUM or METHOD is multivalued it will enumerate all
%   combinations within those parameters. An error will be given if both
%   are multivalued.
%
%   Below is a list of available methods. METHOD can either be written out
%   in full ('Greatest Divisor') or in abbreviated form ('GD'). Also listed
%   are common names for the same method.
%
%   Methods available:
%   ------------------
%   Greatest Divisor   : d'Hondt; Jefferson; Highest Average
%   Smallest Divisor   : Adams
%   Equal Proportions  : Huntington; Hill; Geometric Mean
%   Harmonic Mean      : Dean
%   Major Fractions    : Sainte-Lague; Webster; Arithmetic Mean; Odd Numbers
%   Largest Remainders : Vinton; Hamilton; Hare
%
%   Abbreviated form:
%   -----------------
%   ALL : All six unique methods
%   A   : Adams
%   AM  : Arithmetic Mean
%   D   : Dean
%   DH  : d'Hont
%   EP  : Equal Proportions
%   GD  : Greatest Divisor
%   GM  : Geometric Mean
%   HH  : Huntington OR Hill (equivalent)
%   HA  : Highest Average
%   HAM : Hamilton OR Hare (equivalent)
%   HM  : Harmonic Mean
%   J   : Jefferson
%   LR  : Largest Remainders
%   MF  : Major Fractions
%   ON  : Odd Numbers
%   SD  : Smallest Divisor
%   SL  : Sainte-Lague
%   V   : Vinton
%   W   : Webster
%
%
%   EXAMPLE 1
%   Allocate 80 seats among four parties with votes of [100 80 30 20], with
%   minimum of 3 seats each. Compare all methods.
%      apport([100 80 30 20],80,'All',3)
%
%   EXAMPLE 2
%   Compare method of Greatest Divisor with the method of Smallest Divisor
%   of allocating 15 seats among four parties with votes of [104 62; 34 55]
%       apport([104 62; 34 55],15,{'GD','SD'})
%
%   EXAMPLE 3
%   'Alabama Paradox'
%   "increasing the total number of items would decrease one of the shares"
%      apport([2 6 6],10:13,'Hamilton')
%   Notice first party decreased representation from 2 to 1, even with
%   an increase in total number of shares
%
%   EXAMPLE 4
%   'New states paradox'
%   "it is possible for an existing state to get more representatives than
%   if the new state were not added"
%      apport([27 39 68  ],23,'Largest Remainders')
%      apport([27 39 68 6],23,'Largest Remainders')
%   Notice first state increased representation from 4 to 5, at the
%   expense of one of the other two original states
%

%   Mike Sheppard
%   Last Modified: 1-Feb-2012

%%ERROR CHECKING
if nargin<3
    error('APPORT:TooFewInputs','Too few inputs');
end
if nargin==3
    minseats=0;
end
sz=size(votes); votes=votes(:); minseats=minseats(:);
if isscalar(minseats)
    minseats=minseats.*ones(size(votes)); %make into vector
end
if numel(minseats)~=numel(votes)
    error('APPORT:Number','MINSEATS must be either a scalar or vector of length equal to VOTES');
end
if any(minseats<0)||any(rem(minseats,1))||(any(~isfinite(minseats)))
    error('APPORT:Number','MINSEATS must be a non-negative integers');
end
if (num<0)|(rem(num,1))|(any(~isfinite(num)))
    error('APPORT:Number','NUM must be a positive integer');
end
if (~isnumeric(votes))||(any(~isfinite(votes)))||(any(isnan(votes)))||(~isvector(votes))
    error('APPORT:Votes','VOTES must be a finite numeric value, either scalar or vector');
end
if sum(minseats)>num
    error('APPORT:Number','ERROR: Total minimum allocated seats exceed total seats allowed');
end
method=lower(method);
if ~any(ismember(method,{'all','greatest divisor','dhondt','jefferson',...
        'highest average','smallest divisor','adams','equal proportions',...
        'huntington','hill','geometric mean','harmonic mean','dean',...
        'major fractions','sainte-lague','webster','arithmetic mean',...
        'odd numbers','largest remainders','vinton','hamilton','hare',...
        'a','am','d','dh','ep','gd','gm','hh','ha','ham','hm','j','lr',...
        'mf','on','sd','sl','v','w'}))
    error('APPORT:Method','Method must one of the items in the list under ''help apport''');
end
if iscell(method)&&any(strcmp(method,'all'))
    error('APPORT:Method','METHOD of ALL must be given as a string with no other methods listed');
end
if ischar(method)&&strcmp(method,'all')
    methodv=lower({'Greatest Divisor','Smallest Divisor',...
        'Equal Proportions','Harmonic Mean','Major Fractions',...
        'Largest Remainders'});
elseif ~iscell(method)
    methodv={method}; %Singular method
else
    methodv=method; %Already a cell
end
if all([numel(methodv) numel(num)]>1)
    error('APPORT:Method','NUM and METHOD cannot both be multivalued');
end

nummethods=numel(methodv);
numnum=numel(num);
methodsingular=(nummethods==1);
maxcount=max([nummethods numnum]);

%Save original values
isvec=isvector(ones(sz));

for k=1:maxcount
    if methodsingular
        %methods is singular, loop through num
        seats=apport_helper(votes,num(k),methodv{1},minseats);
    else
        %num is singular, loop through method
        seats=apport_helper(votes,num(1),methodv{k},minseats);
    end
    seats=reshape(seats,sz);

    if maxcount==1
        output=seats; %singular input
    else
        %Multiple inputs
        if ~isvec
            %Multi-dimensional array
            temp(1:ndims(seats))={':'};
            output(temp{:},k)=seats;
        else
            %Vector
            if sz(1)==1
                output(k,:)=seats;
            else
                output(:,k)=seats;
            end
        end
    end

end

%Rename for output
seats=output;

end

function seats=apport_helper(votes,num,method,minseats)
%Subtract minseats from original number
num=num-sum(minseats);

if ismember(method,{'largest remainders','vinton','hamilton','hare','lr','v','ham'})
    %Method of Largest Remainders is handled as specific case
    st_quota=votes./(sum(votes)./num);                                %Standard Quota
    [frac_lost,indx]=sort(rem(st_quota-floor(st_quota),1),'descend'); %Sort by remainders
    seats=floor(st_quota);                                            %Allocate the integer values
    num_missing=num-sum(seats);                                       %Determine number of seats left to allocate
    seats(indx(1:num_missing))=seats(indx(1:num_missing))+1;          %Increase by one seat those with largest remainders
    seats=seats(:);                                                   %Consistent output, column vector
else            %One of the other methods

    %   Algorithm:
    %   Party A with vote total P(A) is entitled to its m'th seat before
    %   party B with vote total P(B) is entitled to its n'th seat
    %   if and only if P(A)/f(m-1) > P(B)/f(n-1) where f(x) is the function
    %   f(x) = x (Smallest Divisor)
    %   f(x) = harmonic mean of x and x + 1 (Harmonic Mean)
    %   f(x) = geometric mean of x and x + 1 (Equal Proportions)
    %   f(x) = arithmetic mean of x and x + 1 (Major Fractions)
    %   f(x) = x + 1 (Greatest Divisor)

    x=[1:num];  %Seat number
    %Note: fd=f(x-0)
    switch lower(method)
        case {'smallest divisor','adams','sd','a'}
            fd=(x);
        case {'harmonic mean','dean','hm','d'}
            fd=harmmean([(x);(x)+1]);
        case {'equal proportions','huntington','hill','geometric mean','ep','hh','gm'}
            fd=geomean([(x);(x)+1]);
        case {'major fractions','sainte-lague','webster', 'arithmetic mean',...
                'odd numbers','mf','sl','w','am','on'}
            fd=mean([(x);(x)+1]);
        case {'greatest divisor','dhondt','jefferson','highest average',...
                'gd','dh','j','ha'}
            fd=(x)+1;
        otherwise
            error('APPORT:Method','Method must one of the items in the list under ''help''');
    end

    %Allocate seats according to priority values
    pv=kron(votes(:),1./fd);               %priority value matrix
    [I,J] = ind2sub(size(pv),1:numel(pv)); %I party index of votes
    [Y,indx] = sort(pv(:),'descend');      %indx is linear index
    %Take the top 'num' of priority values and determine the
    %appropriate party index for each, and group to determine total number of
    %seats for each party
    list=I(indx(1:num));
    seats=accumarray(list',ones(size(list)));

    %Special case to catch:
    %zero seats at end do not show up, add if necessary
    if length(seats)<length(votes)
        seats=[seats; zeros(length(votes)-length(seats),1)];
    end

end

%Add back minimum number of seats and reshape to original shape
seats=seats+minseats;
end


function b = r8bb_vxm ( n1, n2, ml, mu, a, x )

%*****************************************************************************80
%
%% R8BB_VXM multiplies a vector by a R8BB matrix.
%
%  Discussion:
%
%    The R8BB storage format is for a border banded matrix.  Such a
%    matrix has the logical form:
%
%      A1 | A2
%      ---+---
%      A3 | A4
%
%    with A1 a (usually large) N1 by N1 banded matrix, while A2, A3 and A4
%    are dense rectangular matrices of orders N1 by N2, N2 by N1, and N2 by N2,
%    respectively.
%
%    A should be defined as a vector.  The user must then store
%    the entries of the four blocks of the matrix into the vector A.
%    Each block is stored by columns.
%
%    A1, the banded portion of the matrix, is stored in
%    the first (2*ML+MU+1)*N1 entries of A, using standard LINPACK
%    general band format.  The reason for the factor of 2 in front of
%    ML is to allocate space that may be required if pivoting occurs.
%
%    The following formulas should be used to determine how to store
%    the entry corresponding to row I and column J in the original matrix:
%
%    Entries of A1:
%
%      1 <= I <= N1, 1 <= J <= N1, (J-I) <= MU and (I-J) <= ML.
%
%      Store the I, J entry into location
%      (I-J+ML+MU+1)+(J-1)*(2*ML+MU+1).
%
%    Entries of A2:
%
%      1 <= I <= N1, N1+1 <= J <= N1+N2.
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+(J-N1-1)*N1+I.
%
%    Entries of A3:
%
%      N1+1 <= I <= N1+N2, 1 <= J <= N1.
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%
%    Entries of A4:
%
%      N1+1 <= I <= N1+N2, N1+1 <= J <= N1+N2
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%      (same formula used for A3).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N1, N2, the order of the banded and dense blocks
%    N1 and N2 must be nonnegative, and at least one must be positive.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%    ML and MU must be nonnegative and no greater than N1-1.
%
%    Input, real A((2*ML+MU+1)*N1 + 2*N1*N2 + N2*N2), the R8BB matrix.
%
%    Input, real X(N1+N2), the vector to multiply A.
%
%    Output, real B(N1+N2), the product X times A.
%

%
%  Initialize B.
%
  b(1:n1+n2) = 0.0E+00;
%
%  Multiply by A1.
%
  for j = 1 : n1
    ilo = max ( 1, j - mu - ml );
    ihi = min ( n1, j + ml );
    ij = (j-1) * (2*ml+mu+1) - j + ml + mu + 1;
    for i = ilo : ihi
      b(j) = b(j) + x(i) * a(ij+i);
    end
  end
%
%  Multiply by A2.
%
  for j = n1+1 : n1+n2
    ij = (2*ml+mu+1)*n1+(j-n1-1)*n1;
    for i = 1 : n1
      b(j) = b(j) + x(i) * a(ij+i);
    end
  end
%
%  Multiply by A3 and A4.
%
  for j = 1 : n1+n2
    ij = (2*ml+mu+1)*n1+n1*n2+(j-1)*n2-n1;
    for i = n1+1 : n1+n2
      b(j) = b(j) + x(i) * a(ij+i);
    end
  end

  return
end


function mesh = mshCube(N,L)
%+========================================================================+
%|                                                                        |
%|                 OPENMSH - LIBRARY FOR MESH MANAGEMENT                  |
%|           openMsh is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : mshCube.m                                     |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Build uniform mesh for a cube                 |
%|  `---'  |                                                              |
%+========================================================================+

% Optimal number of point for each dimension
n = L/min(L);
n = round( n * (N/prod(n))^(1/3) );

% Delaunay mesh
x = -0.5*L(1) : L(1)/n(1) : 0.5*L(1);
y = -0.5*L(2) : L(2)/n(2) : 0.5*L(2);
z = -0.5*L(3) : L(3)/n(3) : 0.5*L(3);
[x,y,z] = meshgrid(x,y,z);
DT      = delaunayTriangulation([x(:) y(:) z(:)]);

% Build mesh
mesh = msh(DT.Points,DT.ConnectivityList);
end


function r = tanh(a)
%TANH          Taylor hyperbolic tangent  tanh(a)
%
%Thanks to George Corliss for providing the Taylor expansion
%

% written  06/03/09     S.M. Rump
% 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 = getappdata(0,'INTLAB_TAYLOR_ORDER');

  r = a;
  ct = a.t;
  N = size(a.t,2);
  r.t(1,:) = tanh(a.t(1,:));
  ct(1,:) = 1-r.t(1,:).^2;     % 1-tanh(a)^2
  r.t(2,:) = ct(1,:) .* a.t(2,:);
  for j=2:K
    ct(j,:) = -sum( r.t(1:j,:).*r.t(j:-1:1,:) , 1 );
    r.t(j+1,:) = sum( ct(1:j,:).*a.t(j+1:-1:2,:).*repmat((j:-1:1)',1,N) , 1 )/j;
  end

  if rndold
    setround(rndold)
  end


function f = p17_f ( n, x )

%*****************************************************************************80
%
%% P17_F evaluates the objective function for problem 17.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 January 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Richard Brent,
%    Algorithms for Minimization with Derivatives,
%    Dover, 2002,
%    ISBN: 0-486-41998-3,
%    LC: QA402.5.B74.
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  f1 = x(2) - x(1) * x(1);
  f2 = 1.0 - x(1);
  f3 = x(4) - x(3) * x(3);
  f4 = 1.0 - x(3);
  f5 = x(2) + x(4) - 2.0;
  f6 = x(2) - x(4);

  f = 100.0 * f1 * f1 ...
    +         f2 * f2 ...
    +  90.0 * f3 * f3 ...
    +         f4 * f4 ...
    +  10.0 * f5 * f5 ...
    +   0.1 * f6 * f6;

  return
end


function [ p ] = normalize_vec( x )
p = x / sum(x);
end


function C=multiply4X5And5X5Matrices(A,B)
%%MULTIPLY4X4And5X5Matrices This just multiplies the 4X4 matrix A by the
%           5X5 matrix B to get a 4X5 matrix C. This demonstrates the
%           algorithm developed in [1], which minimizes the number of
%           scalar multiplication operations. This could be used as a
%           template for efficient implementation in other programming
%           languages. In Matlab, this is not faster than the built-in
%           matrix multiplication operation.
%
%INPUTS: A A 4X5 matrix.
%        B A 5X5 matrix.
%
%OUTPUTS: C A 4X5 matrix.
%
%EXAMPLE:
%This just shows that this function produces the same result as Matlab
%within finite precision limitations.
% A=randn(4,5);
% B=randn(5,5);
% C=multiply4X5And5X5Matrices(A,B);
% C1=A*B;%Matlab's way.
% RelErr=max(max(abs(abs((C-C1)./C1))))
%
%REFERENCES:
%[1] A. Fawzi, M. Balog, A. Huang, T. Hubert, B. Romera-Paredes,
%    M. Barekatain, A. Novikov, F. J. R. Ruiz, J. Schrittwieser, G.
%    Swirszcz, D. Silver, D. Hassabis, and P. Kohli, "Discovering faster
%    matrix multiplication algorithms with reinforcement learning," Nature,
%    vol. 610, pp. 47-53, 2022.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

h1=A(3,2)*(-B(2,1)-B(2,5)-B(3,1));
h2=(A(2,2)+A(2,5)-A(3,5))*(-B(2,5)-B(5,1));
h3=(-A(3,1)-A(4,1)+A(4,2))*(-B(1,1)+B(2,5));
h4=(A(1,2)+A(1,4)+A(3,4))*(-B(2,5)-B(4,1));
h5=(A(1,5)+A(2,2)+A(2,5))*(-B(2,4)+B(5,1));
h6=(-A(2,2)-A(2,5)-A(4,5))*(B(2,3)+B(5,1));
h7=(-A(1,1)+A(4,1)-A(4,2))*(B(1,1)+B(2,4));
h8=(A(3,2)-A(3,3)-A(4,3))*(-B(2,3)+B(3,1));
h9=(-A(1,2)-A(1,4)+A(4,4))*(B(2,3)+B(4,1));
h10=(A(2,2)+A(2,5))*B(5,1);
h11=(-A(2,1)-A(4,1)+A(4,2))*(-B(1,1)+B(2,2));
h12=(A(4,1)-A(4,2))*B(1,1);
h13=(A(1,2)+A(1,4)+A(2,4))*(B(2,2)+B(4,1));
h14=(A(1,3)-A(3,2)+A(3,3))*(B(2,4)+B(3,1));
h15=(-A(1,2)-A(1,4))*B(4,1);
h16=(-A(3,2)+A(3,3))*B(3,1);
h17=(A(1,2)+A(1,4)-A(2,1)+A(2,2)-A(2,3)+A(2,4)-A(3,2)+A(3,3)-A(4,1)+A(4,2))*B(2,2);
h18=A(2,1)*(B(1,1)+B(1,2)+B(5,2));
h19=-A(2,3)*(B(3,1)+B(3,2)+B(5,2));
h20=(-A(1,5)+A(2,1)+A(2,3)-A(2,5))*(-B(1,1)-B(1,2)+B(1,4)-B(5,2));
h21=(A(2,1)+A(2,3)-A(2,5))*B(5,2);
h22=(A(1,3)-A(1,4)-A(2,4))*(B(1,1)+B(1,2)-B(1,4)-B(3,1)-B(3,2)+B(3,4)+B(4,4));
h23=A(1,3)*(-B(3,1)+B(3,4)+B(4,4));
h24=A(1,5)*(-B(4,4)-B(5,1)+B(5,4));
h25=-A(1,1)*(B(1,1)-B(1,4));
h26=(-A(1,3)+A(1,4)+A(1,5))*B(4,4);
h27=(A(1,3)-A(3,1)+A(3,3))*(B(1,1)-B(1,4)+B(1,5)+B(3,5));
h28=-A(3,4)*(-B(3,5)-B(4,1)-B(4,5));
h29=A(3,1)*(B(1,1)+B(1,5)+B(3,5));
h30=(A(3,1)-A(3,3)+A(3,4))*B(3,5);
h31=(-A(1,4)-A(1,5)-A(3,4))*(-B(4,4)-B(5,1)+B(5,4)-B(5,5));
h32=(A(2,1)+A(4,1)+A(4,4))*(B(1,3)-B(4,1)-B(4,2)-B(4,3));
h33=A(4,3)*(-B(3,1)-B(3,3));
h34=A(4,4)*(-B(1,3)+B(4,1)+B(4,3));
h35=-A(4,5)*(B(1,3)+B(5,1)+B(5,3));
h36=(A(2,3)-A(2,5)-A(4,5))*(B(3,1)+B(3,2)+B(3,3)+B(5,2));
h37=(-A(4,1)-A(4,4)+A(4,5))*B(1,3);
h38=(-A(2,3)-A(3,1)+A(3,3)-A(3,4))*(B(3,5)+B(4,1)+B(4,2)+B(4,5));
h39=(-A(3,1)-A(4,1)-A(4,4)+A(4,5))*(B(1,3)+B(5,1)+B(5,3)+B(5,5));
h40=(-A(1,3)+A(1,4)+A(1,5)-A(4,4))*(-B(3,1)-B(3,3)+B(3,4)+B(4,4));
h41=(-A(1,1)+A(4,1)-A(4,5))*(B(1,3)+B(3,1)+B(3,3)-B(3,4)+B(5,1)+B(5,3)-B(5,4));
h42=(-A(2,1)+A(2,5)-A(3,5))*(-B(1,1)-B(1,2)-B(1,5)+B(4,1)+B(4,2)+B(4,5)-B(5,2));
h43=A(2,4)*(B(4,1)+B(4,2));
h44=(A(2,3)+A(3,2)-A(3,3))*(B(2,2)-B(3,1));
h45=(-A(3,3)+A(3,4)-A(4,3))*(B(3,5)+B(4,1)+B(4,3)+B(4,5)+B(5,1)+B(5,3)+B(5,5));
h46=-A(3,5)*(-B(5,1)-B(5,5));
h47=(A(2,1)-A(2,5)-A(3,1)+A(3,5))*(B(1,1)+B(1,2)+B(1,5)-B(4,1)-B(4,2)-B(4,5));
h48=(-A(2,3)+A(3,3))*(B(2,2)+B(3,2)+B(3,5)+B(4,1)+B(4,2)+B(4,5));
h49=(-A(1,1)-A(1,3)+A(1,4)+A(1,5)-A(2,1)-A(2,3)+A(2,4)+A(2,5))*(-B(1,1)-B(1,2)+B(1,4));
h50=(-A(1,4)-A(2,4))*(B(2,2)-B(3,1)-B(3,2)+B(3,4)-B(4,2)+B(4,4));
h51=A(2,2)*(B(2,1)+B(2,2)-B(5,1));
h52=A(4,2)*(B(1,1)+B(2,1)+B(2,3));
h53=-A(1,2)*(-B(2,1)+B(2,4)+B(4,1));
h54=(A(1,2)+A(1,4)-A(2,2)-A(2,5)-A(3,2)+A(3,3)-A(4,2)+A(4,3)-A(4,4)-A(4,5))*B(2,3);
h55=(A(1,4)-A(4,4))*(-B(2,3)+B(3,1)+B(3,3)-B(3,4)+B(4,3)-B(4,4));
h56=(A(1,1)-A(1,5)-A(4,1)+A(4,5))*(B(3,1)+B(3,3)-B(3,4)+B(5,1)+B(5,3)-B(5,4));
h57=(-A(3,1)-A(4,1))*(-B(1,3)-B(1,5)-B(2,5)-B(5,1)-B(5,3)-B(5,5));
h58=(-A(1,4)-A(1,5)-A(3,4)-A(3,5))*(-B(5,1)+B(5,4)-B(5,5));
h59=(-A(3,3)+A(3,4)-A(4,3)+A(4,4))*(B(4,1)+B(4,3)+B(4,5)+B(5,1)+B(5,3)+B(5,5));
h60=(A(2,5)+A(4,5))*(B(2,3)-B(3,1)-B(3,2)-B(3,3)-B(5,2)-B(5,3));
h61=(A(1,4)+A(3,4))*(B(1,1)-B(1,4)+B(1,5)-B(2,5)-B(4,4)+B(4,5)-B(5,1)+B(5,4)-B(5,5));
h62=(A(2,1)+A(4,1))*(B(1,2)+B(1,3)+B(2,2)-B(4,1)-B(4,2)-B(4,3));
h63=(-A(3,3)-A(4,3))*(-B(2,3)-B(3,3)-B(3,5)-B(4,1)-B(4,3)-B(4,5));
h64=(A(1,1)-A(1,3)-A(1,4)+A(3,1)-A(3,3)-A(3,4))*(B(1,1)-B(1,4)+B(1,5));
h65=(-A(1,1)+A(4,1))*(-B(1,3)+B(1,4)+B(2,4)-B(5,1)-B(5,3)+B(5,4));
h66=(A(1,1)-A(1,2)+A(1,3)-A(1,5)-A(2,2)-A(2,5)-A(3,2)+A(3,3)-A(4,1)+A(4,2))*B(2,4);
h67=(A(2,5)-A(3,5))*(B(1,1)+B(1,2)+B(1,5)-B(2,5)-B(4,1)-B(4,2)-B(4,5)+B(5,2)+B(5,5));
h68=(A(1,1)+A(1,3)-A(1,4)-A(1,5)-A(4,1)-A(4,3)+A(4,4)+A(4,5))*(-B(3,1)-B(3,3)+B(3,4));
h69=(-A(1,3)+A(1,4)-A(2,3)+A(2,4))*(-B(2,4)-B(3,1)-B(3,2)+B(3,4)-B(5,2)+B(5,4));
h70=(A(2,3)-A(2,5)+A(4,3)-A(4,5))*(-B(3,1)-B(3,2)-B(3,3));
h71=(-A(3,1)+A(3,3)-A(3,4)+A(3,5)-A(4,1)+A(4,3)-A(4,4)+A(4,5))*(-B(5,1)-B(5,3)-B(5,5));
h72=(-A(2,1)-A(2,4)-A(4,1)-A(4,4))*(B(4,1)+B(4,2)+B(4,3));
h73=(A(1,3)-A(1,4)-A(1,5)+A(2,3)-A(2,4)-A(2,5))*(B(1,1)+B(1,2)-B(1,4)+B(2,4)+B(5,2)-B(5,4));
h74=(A(2,1)-A(2,3)+A(2,4)-A(3,1)+A(3,3)-A(3,4))*(B(4,1)+B(4,2)+B(4,5));
h75=-(A(1,2)+A(1,4)-A(2,2)-A(2,5)-A(3,1)+A(3,2)+A(3,4)+A(3,5)-A(4,1)+A(4,2))*B(2,5);
h76=(A(1,3)+A(3,3))*(-B(1,1)+B(1,4)-B(1,5)+B(2,4)+B(3,4)-B(3,5));

C=zeros(4,5);
C(1,1)=-h10+h12+h14-h15-h16+h53+h5-h66-h7;
C(2,1)=h10+h11-h12+h13+h15+h16-h17-h44+h51;
C(3,1)=h10-h12+h15+h16-h1+h2+h3-h4+h75;
C(4,1)=-h10+h12-h15-h16+h52+h54-h6-h8+h9;
C(1,2)=h13+h15+h20+h21-h22+h23+h25-h43+h49+h50;
C(2,2)=-h11+h12-h13-h15-h16+h17+h18-h19-h21+h43+h44;
C(3,2)=-h16-h19-h21-h28-h29-h38+h42+h44-h47+h48;
C(4,2)=h11-h12-h18+h21-h32+h33-h34-h36+h62-h70;
C(1,3)=h15+h23+h24+h34-h37+h40-h41+h55-h56-h9;
C(2,3)=-h10+h19+h32+h35+h36+h37-h43-h60-h6-h72;
C(3,3)=-h16-h28+h33+h37-h39+h45-h46+h63-h71-h8;
C(4,3)=h10+h15+h16-h33+h34-h35-h37-h54+h6+h8-h9;
C(1,4)=-h10+h12+h14-h16+h23+h24+h25+h26+h5-h66-h7;
C(2,4)=h10+h18-h19+h20-h22-h24-h26-h5-h69+h73;
C(3,4)=-h14+h16-h23-h26+h27+h29+h31+h46-h58+h76;
C(4,4)=h12+h25+h26-h33-h35-h40+h41+h65-h68-h7;
C(1,5)=h15+h24+h25+h27-h28+h30+h31-h4+h61+h64;
C(2,5)=-h10-h18-h2-h30-h38+h42-h43+h46+h67+h74;
C(3,5)=-h10+h12-h15+h28+h29-h2-h30-h3+h46+h4-h75;
C(4,5)=-h12-h29+h30-h34+h35+h39+h3-h45+h57+h59;

end

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


function [spec,idctm] = cep2spec(cep, nfreq, type)
% spec = cep2spec(cep, nfreq, type)
%   Reverse the cepstrum to recover a spectrum.
%   i.e. converse of spec2cep
%   nfreq is how many points to reconstruct in spec
% 2005-05-15 dpwe@ee.columbia.edu

if nargin < 2;   nfreq = 21;   end
if nargin < 3;   type = 2;   end   % type of DCT

[ncep,ncol] = size(cep);

% Make the DCT matrix
dctm = zeros(ncep, nfreq);
idctm = zeros(nfreq, ncep);
if type == 2 || type == 3
  % this is the orthogonal one, so inv matrix is same as fwd matrix
  for i = 1:ncep
    dctm(i,:) = cos((i-1)*[1:2:(2*nfreq-1)]/(2*nfreq)*pi) * sqrt(2/nfreq);
  end
  if type == 2
    % make it unitary! (but not for HTK type 3)
    dctm(1,:) = dctm(1,:)/sqrt(2);
  else
    dctm(1,:) = dctm(1,:)/2;
  end
  idctm = dctm';
elseif type == 4 % type 1 with implicit repetition of first, last bins
  % so all we do is reconstruct the middle nfreq rows of an nfreq+2 row idctm
  for i = 1:ncep
    % 2x to compensate for fact that only getting +ve freq half
    idctm(:,i) = 2*cos((i-1)*[1:nfreq]'/(nfreq+1)*pi);
  end
  % fixup 'non-repeated' basis fns
  idctm(:, [1 ncep]) = idctm(:, [1 ncep])/2;
else % dpwe type 1 - idft of cosine terms
  for i = 1:ncep
    % 2x to compensate for fact that only getting +ve freq half
    idctm(:,i) = 2*cos((i-1)*[0:(nfreq-1)]'/(nfreq-1)*pi);
  end
  % fixup 'non-repeated' basis fns
  idctm(:, [1 ncep]) = 0.5* idctm(:, [1 ncep]);
end

spec = exp(idctm*cep);


function X = MultiSolver(M, RHS)
% function X = MultiSolver(M, RHS)
%
% PURPOSE: Multiple linear (least-square) solver (usually small) for
%          systems having the same size
%
% INPUT
%   M  : 3D array (m x n x p)
%   RHS: 3D array (m x p x q)
% OUTPUT
%   X  : 3D array (n x p x q) if p>1,
%      : 2D array (n x q) if p==1 (squeezed, and equal to standard
%        backslash M\RHS)
%
% Solve p systems of linear equations:
%   M(:,:,k) * squeeze(X(:,k,:)) = squeeze(RHS(:,k,:)) for all k=1,2,...,p
%
% NOTE 1 -- This function is likely used with q == 1: single RHS for each
%           system). In this case
%       M  : 3D array (m x n x p)
%       RHS: 2D array (m x p)
%       X  : 2D array (n x p)
%       M(:,:,k) * X(:,k) = RHS(:,k) for all k=1,2,...,p
%
% NOTE 2 -- Special call with squeezed RHS: common RHS for all systems.
%   Input argument RHS can be squeezed as (m x q), or leave as 3D with
%   singleton in the second dimension: (m x 1 x q).
%       M   is 3D array (m x n x p)
%       RHS is 2D array (m x q) or (m x 1 x q)
%       X   is 2D array (n x p x q)
%       M(:,:,k) * squeeze(X(:,k,:)) = squeeze(RHS) for all k=1,2,...,p
%
% NOTE  3 -- Underdetermined system (more unknowns than equation)
%   The solution is basic solution obtained with sparse mldivide
%   which is not the same as basic solution when calling for full matrix.
%
% See also: SliceMultiSolver, MultiProd
%
% Author: Bruno Luong <brunoluong@yahoo.com>
% History: original 26-May-2009
%          11-Auguts-2009, add help
% Acknowlegement: Tim Davis for the tip of using sparse to speedup
%                 from FOR loop method

if ndims(M)>3 || ndims(RHS)>3
    error('MultiSolver: M or RHS cannot have more than 3 dimensions');
end

[m n p] = size(M);

if size(RHS,1)~=m
    error('MultiSolver: M and RHS dimensions are not compatible');
end

if (size(RHS,2)~=p)
    if ndims(RHS)==2 % (m x q)
        RHS = reshape(RHS, m, 1, []);
    elseif size(RHS,2)~=1
        error('MultiSolver: M and RHS dimensions are not compatible');
    end
    % common RHS, expanded for all systems
    RHS = repmat(RHS, [1 p 1]); % (m x p x q)
end

RHS = reshape(RHS, m*p, []); % (m*p) x q
q = size(RHS,2);

% Build sparse matrix and solve
I = repmat(reshape(1:m*p,m,1,p),[1 n 1]); % m x n x p
J = repmat(reshape(1:n*p,1,n,p),[m 1 1]); % m x n x p
A = sparse(I(:),J(:),M(:));
X = reshape(A \ RHS, [n p q]);

% Squeeze for single system
if p==1
    X = reshape(X, [n q]);
end

end % MultiSolver


function value = filon_tab_cos ( n, ftab, a, b, t )

%*****************************************************************************80
%
%% FILON_TAB_COS uses Filon's method on integrals with a cosine factor.
%
%  Discussion:
%
%    The integral to be approximated has the form:
%
%      Integral ( A <= X <= B ) F(X) * COS(T*X) dX
%
%    where T is user specified.
%
%    The function is interpolated over each subinterval by
%    a parabolic arc.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Chase, Lloyd Fosdick,
%    An Algorithm for Filon Quadrature,
%    Communications of the Association for Computing Machinery,
%    Volume 12, Number 8, August 1969, pages 453-457.
%
%    Stephen Chase, Lloyd Fosdick,
%    Algorithm 353:
%    Filon Quadrature,
%    Communications of the Association for Computing Machinery,
%    Volume 12, Number 8, August 1969, pages 457-458.
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer N, the number of data points.
%    N must be odd, and greater than 1.
%
%    Input, real FTAB(N), contains the value of the function
%    at A, A+H, A+2*H, ... , B-H, B, where H = (B-A)/(N-1).
%
%    Input, real A, B, the limits of integration.
%
%    Input, real T, the multiplier of the X argument of the cosine.
%
%    Output, real VALUE, the approximate value of the integral.
%
  if ( a == b )
    value = 0.0;
    return
  end

  if ( n <= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILON_TAB_COS - Fatal error!\n' );
    fprintf ( 1, '  N < 2\n' );
    fprintf ( 1, '  N = %d\n', n );
    error ( 'FILON_TAB_COS - Fatal error!' );
  end

  if ( mod ( n, 2 ) ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILON_TAB_COS - Fatal error!\n' );
    fprintf ( 1, '  N must be odd.\n' );
    fprintf ( 1, '  N = %d\n', n );
    error ( 'FILON_TAB_COS - Fatal error!' );
  end
%
%  Set the X values.
%
  x = linspace ( a, b, n );
  h = ( b - a ) / ( n - 1 );

  theta = t * h;
  sint = sin ( theta );
  cost = cos ( theta );

  if ( 6.0 * abs ( theta ) <= 1.0 )

    alpha = 2.0 * theta^3 /   45.0 ...
          - 2.0 * theta^5 /  315.0 ...
          + 2.0 * theta^7 / 4725.0;

    beta =  2.0           /     3.0 ...
          + 2.0 * theta^2 /    15.0 ...
          - 4.0 * theta^4 /   105.0 ...
          + 2.0 * theta^6 /   567.0 ...
          - 4.0 * theta^8 / 22275.0;

    gamma = 4.0           /      3.0 ...
          - 2.0 * theta^2 /     15.0 ...
          +       theta^4 /    210.0 ...
          -       theta^6 /  11340.0;

  else

    alpha = ( theta^2 + theta * sint * cost - 2.0 * sint^2 ) / theta^3;

    beta = ( 2.0 * theta + 2.0 * theta * cost^2 ...
      - 4.0 * sint * cost ) / theta^3;

    gamma = 4.0 * ( sint - theta * cost ) / theta^3;

  end

  c2n = sum ( ftab(1:2:n) .* cos ( t * x(1:2:n) ) ) ...
    - 0.5 * ( ftab(n) * cos ( t * x(n) ) ...
            + ftab(1) * cos ( t * x(1) ) );

  c2nm1 = sum ( ftab(2:2:n-1) .* cos ( t * x(2:2:n-1) ) );

  value = h * ( ...
      alpha * ( ftab(n) * sin ( t * x(n) ) ...
              - ftab(1) * sin ( t * x(1) ) ) ...
    + beta * c2n ...
    + gamma * c2nm1 );

  return
end


function [pol, res, zer] = prz(zj, fj, wj)
%PRZ   Computes poles, residues, and zeros of a rational function in
%      barycentric form.
%   [POL, RES, ZER] = PRZ(ZJ, FJ, WJ) returns vectors of poles POL,
%   residues RES, and zeros ZER of the rational function defined by
%   support points ZJ, function values FJ, and barycentric weights WJ.
%
% See also AAA, PRZTRIG, REVAL.

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

m = length(wj);

% Compute poles via generalized eigenvalue problem:
B = eye(m+1);
B(1,1) = 0;
E = [0 wj.'; ones(m, 1) diag(zj)];
pol = eig(E, B);
% Remove zeros of denominator at infinity:
pol = pol(~isinf(pol));

% Compute residues via formula for res of quotient of analytic functions:
N = @(t)(1./bsxfun(@minus,t,zj.')) * (fj.*wj);
Ddiff = @(t) -((1./bsxfun(@minus,t,zj.')).^2) * wj;
res = N(pol)./Ddiff(pol);

% Compute zeros via generalized eigenvalue problem:
E = [0 (wj.*fj).'; ones(m, 1) diag(zj)];
zer = eig(E, B);
% Remove zeros of numerator at infinity:
zer = zer(~isinf(zer));

end % End of PRZ().


% ---------------------------------------------------------------------
% In this script trajectory optimization otherwise called experiment
% design for dynamic paramters identification is carried out.
%
% First, specify cost function (traj_cost_lgr) and constraints
% (traj_cnstr) for the optimziation. Then choose oprimization algorithm and
% specify trajectory parameters (duration, fundamental frequency, number of
% harmonics, initial (= final) positionin) and max/min positions,
% velocities and accelerations.
%
% Then script runs optimization, plots obtained trajectory and saves its
% parameters into a file.
% ---------------------------------------------------------------------
% get robot description
path_to_urdf = 'ur10e.urdf';
ur10 = parse_urdf(path_to_urdf);

% get mapping from full parameters to base parameters
include_motor_dynamics = 1;
[~, baseQR] = base_params_qr(include_motor_dynamics);

% Choose optimization algorithm: 'patternsearch', 'ga'
optmznAlgorithm = 'patternsearch';

% Trajectory parameters
traj_par.T = 25;          % period of signal
traj_par.wf = 2*pi/traj_par.T;    % fundamental frequency
traj_par.t_smp = 2e-1;   % sampling time
traj_par.t = 0:traj_par.t_smp:traj_par.T;  % time
traj_par.N = 7;          % number of harmonics
traj_par.q0 = deg2rad([0 -90 0 -90 0 0 ]');
% Use different limit for positions for safety
traj_par.q_min = -deg2rad([180  180  100   180  90   90]');
traj_par.q_max =  deg2rad([180  0    100   0    90   90]');
traj_par.qd_max = qd_max;
traj_par.q2d_max = [2 1 1 1 1 2.5]';

%  ----------------------------------------------------------------------
% Otimization
% -----------------------------------------------------------------------
A = []; b = [];
Aeq = []; beq = [];
lb = []; ub = [];

if strcmp(optmznAlgorithm, 'patternsearch')
    x0 = rand(6*2*traj_par.N,1);
    optns_pttrnSrch = optimoptions('patternsearch');
    optns_pttrnSrch.Display = 'iter';
    optns_pttrnSrch.StepTolerance = 1e-1;
    optns_pttrnSrch.FunctionTolerance = 10;
    optns_pttrnSrch.ConstraintTolerance = 1e-6;
    optns_pttrnSrch.MaxTime = inf;
    optns_pttrnSrch.MaxFunctionEvaluations = 1e+6;

    [x,fval] = patternsearch(@(x)traj_cost_lgr(x,traj_par,baseQR), x0, ...
                             A, b, Aeq, beq, lb, ub, ...
                             @(x)traj_cnstr(x,traj_par), optns_pttrnSrch);
elseif strcmp(optmznAlgorithm, 'ga')
    optns_ga = optimoptions('ga');
    optns_ga.Display = 'iter';
    optns_ga.PlotFcn = 'gaplotbestf'; % {'gaplotbestf', 'gaplotscores'}
    optns_ga.MaxGenerations = 50;
    optns_ga.PopulationSize = 1e+3; % in each generation.
    optns_ga.InitialPopulationRange = [-100; 100];
    optns_ga.SelectionFcn = 'selectionroulette';

    [x,fval] = ga(@(x)traj_cost_lgr(x,traj_par,baseQR), 6*2*traj_par.N,...
                  A, b, Aeq, beq, lb, ub, ...
                  @(x)traj_cnstr(x,traj_par), optns_ga);
else
    error('Chosen algorithm is not found among implemented ones');
end

% ------------------------------------------------------------------------
% Plotting obtained trajectory
% ------------------------------------------------------------------------
ab = reshape(x,[12,traj_par.N]);
a = ab(1:6,:); % sin coeffs
b = ab(7:12,:); % cos coeffs
c_pol = getPolCoeffs(traj_par.T, a, b, traj_par.wf, traj_par.N, traj_par.q0);
[q,qd,q2d] = mixed_traj(traj_par.t, c_pol, a, b, traj_par.wf, traj_par.N);

figure
subplot(3,1,1)
    plot(traj_par.t,q)
    ylabel('$q$','interpreter','latex')
    grid on
    legend('q1','q2','q3','q4','q5','q6')
subplot(3,1,2)
    plot(traj_par.t,qd)
    ylabel('$\dot{q}$','interpreter','latex')
    grid on
    legend('qd1','qd2','qd3','qd4','qd5','qd6')
subplot(3,1,3)
    plot(traj_par.t,q2d)
    ylabel('$\ddot{q}$','interpreter','latex')
    grid on
    legend('q2d1','q2d2','q2d3','q2d4','q2d5','q2d6')

% ----------------------------------------------------------------------
% Saving parameters of the optimized trajectory
% ----------------------------------------------------------------------
% %{
pathToFolder = 'trajectory_optmzn/optimal_trjctrs/';
t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
if strcmp(optmznAlgorithm, 'patternsearch')
    filename = strcat(pathToFolder,'ptrnSrch_',t1,'QR.mat');
elseif strcmp(optmznAlgorithm, 'ga')
    filename = strcat(pathToFolder,'ga_',t1,'.mat');
elseif strcmp(optmznAlgorithm, 'fmincon')
    filename = strcat(pathToFolder,'fmncn_',t1,'.mat');
end
save(filename,'a','b','c_pol','traj_par')
%}


function [alpha, E, A] = inexact_alm_rlr(X, K, D, tol, maxIter)

% This matlab code implements the inexact augmented Lagrange multiplier
% method for Robust linear regresion with known alpha
%
% alpha- k x n matrix of known coefficients
%
% X - m x n matrix of observations/data (required input)
%
% tol - tolerance for stopping criterion.
%
% maxIter - maximum number of iterations
%
%
% min_D \|E\|_1, s.t. X = D*alpha +E;
%
% The laplacian function: L(A,E,Y,u) =  |E|_1  + \mu/2* |X- D*alpha-E+Y/mu|_F^2;
%
%
% Xianbiao Shu (xshu2@illinois.edu)
% Copyright: Mitsubishi Electric Research Lab
% Reference: X. Shu, F. Porikli, N. Ahujia "Robust Orthonormal Subspace Learning: Efficient Recovery of Corrupted Low-rank Matrices". CVPR 2014;

addpath PROPACK;

[m, n] = size(X);

% initialize
% alpha   % Low-rank coeffs

E = zeros(size(X)); % sparse innovation
A = X;

Z = 0; % Z = Y/mu

norm_inf = norm(X(:), inf);
d_norm = norm(X, 'fro');

mu = 10*5e-2/norm_inf; % this parameter can be tuned
rho = 1.5;       %1.3    % this parameter can be tuned
mu_bar = mu * 1e7;

iter = 0;
Iteration = 0;
converged = false;

while ~converged

    iter = iter + 1;

    E_temp = X - A + Z;

    %% Intensity threshold
    E = max(abs(E_temp) - 1/mu, 0).*sign(E_temp);

    %% Sparse Representation
    A_ini = X -E + Z;

%%   given D and A

    [U, S, V] = svd(D, 0);
    diag_S = diag(S);
    SV = length(diag_S(diag_S>0));

    alpha = V(:, 1:SV)*diag(1./diag_S(1:SV))*(U(:, 1:SV))'*A_ini;
    A = D*alpha;

    Iteration = Iteration + 1;

    Error = X - A - E;
    Z = (Z + Error)/rho;

    mu = min(mu*rho, mu_bar);

    %% stop Criterion
    stopCriterion = norm(Error, 'fro') / d_norm;
    if stopCriterion < tol || iter >= maxIter
        converged = true;
    end

     disp(['#Iter ' num2str(Iteration)  ...
            ' |E|_0 ' num2str(length(find(abs(E)>0)))...
            ' stopCriterion ' num2str(stopCriterion)]);

end


% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function Q = rand_ising_grid(n_vars)
% RAND_ISING: Function generates a random interaction matrix
% for an Ising Model on a grid graph with n_vars total variables

% Check that n_side is an integer
n_side = sqrt(n_vars);
if floor(n_side) ~= n_side
	error('Number of nodes is not square')
end

% Connect nodes horizontally
for i=1:n_side
	for j=1:n_side-1

		% Determine node idx
		node = (i-1)*n_side + j;

		Q(node,node+1) = 4.95*rand() + 0.05;%0.95*rand() + 0.05;
		Q(node+1,node) = Q(node,node+1);

	end
end

% Connect nodes vertically
for i=1:n_side-1
	for j=1:n_side

		% Determine node idx
		node = (i-1)*n_side + j;

		Q(node,node+n_side) = 4.95*rand() + 0.05;%0.95*rand() + 0.05;
		Q(node+n_side,node) = Q(node,node+n_side);

	end
end

% Apply random sign flips to Q
rand_sign = tril((rand(n_vars,n_vars) > 0.5)*2 - 1,-1);
rand_sign = rand_sign + rand_sign';
Q = rand_sign.*Q;

end