%+========================================================================+
%|                                                                        |
%|            This script uses 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       : nrtHelmholtz2dSDrad.m                         |
%|    #    |   VERSION    : 0.50                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & Martin Averseng             |
%|  ( # )  |   CREATION   : 25.11.2018                                    |
%|  / 0 \  |   LAST MODIF :                                               |
%| ( === ) |   SYNOPSIS   : Solve dirichlet scatering problem with single |
%|  `---'  |                layer potential                               |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
N   = 5e2
typ = 'P1'
gss = 3
X0  = zeros(1,3)
k   = 5

% Boundary mesh
mesh = mshCircle(N,1);

% Ellipse
% mesh.vtx(:,1) = 0.7*mesh.vtx(:,1);
% mesh.vtx(:,2) = 0.5*mesh.vtx(:,2);

% Radiative mesh
radiat = mshSquare(10*N,[5 5]);

% Domain
sigma = dom(mesh,gss);

% Finite element
u = fem(mesh,typ);

% Mesh representation
figure
plot(mesh)
hold on
plotNrm(mesh)
plot(radiat,'w')
plot(sigma)
plot(u)
hold off
axis equal
axis(2.5*[-1 1 -1 1 -1 1])
alpha(0.5)
view(0,90)

% Spherical sources
SW      = @(X) femGreenKernel(X,X0,'[H0(kr)]',k);
dxSW{1} = @(X) femGreenKernel(X,X0,'gradx[H0(kr)]1',k);
dxSW{2} = @(X) femGreenKernel(X,X0,'gradx[H0(kr)]2',k);
dxSW{3} = @(X) femGreenKernel(X,X0,'gradx[H0(kr)]3',k);

% Green kernels
Gxy      = @(X,Y) femGreenKernel(X,Y,'[H0(kr)]',k);
dyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]1',k);
dyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]2',k);
dyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]3',k);

% Jump trace : mu = [p] = pi - pe    lambda = [dnp] = dnpi - dnpe
mu     = - integral(sigma,u,SW);    % pi = 0
lambda = - integral(sigma,ntimes(u),dxSW);  % dnpi = 0

% Validation for mu
sol = -sum(mu);
ref = besselh(0,k) * 2*pi;
norm(ref-sol)/norm(ref)

% Validation for lambda
sol = -sum(lambda);
ref = - k*besselh(1,k) * 2*pi;
norm(ref-sol)/norm(ref)

% Mass matrix
Id = integral(sigma,u,u);

% Single Layer
tic
S   = 1i/4 .* integral(radiat.vtx,sigma,Gxy,u);
Sr  = -1/(2*pi) .* regularize(radiat.vtx,sigma,'[log(r)]',u);
S   = S + Sr;
toc

% Double layer
tic
D  = 1i/4 .* integral(radiat.vtx,sigma,dyGxy,ntimes(u));
Dr = -1/(2*pi) .* regularize(radiat.vtx,sigma,'grady[log(r)]',ntimes(u));
D  = D + Dr;
toc

% Radiation with integral representation
sol = S * (Id\lambda) - D * (Id\mu);

% Analytic solution : Gk(X,X0) exterieur et 0 interieur
ref = SW(radiat.vtx);
ind = find(sqrt(sum(radiat.vtx.^2,2)) >= 1.01);
norm(ref(ind)-sol(ind))/norm(ref(ind))
norm(ref(ind)-sol(ind),'inf')/norm(ref(ind),'inf')

% Graphical representation
theta = 0:2*pi/1e3:2*pi;
figure
plot(radiat,real(ref))
hold on
plot(mesh,'k')
hold off
title('Analytic solution')
xlabel('X');   ylabel('Y');   zlabel('Z');
hold off
colorbar
caxis([-1 1])
axis equal
axis(2.5*[-1 1 -1 1 -1 1])
view(0,90)

figure
plot(radiat,real(sol))
hold on
plot(mesh,'k')
hold off
title('Radiation solution')
xlabel('X');   ylabel('Y');   zlabel('Z');
hold off
colorbar
caxis([-1 1])
axis equal
axis(2.5*[-1 1 -1 1 -1 1])
view(0,90)

% Single Layer
tic
S  = 1i/4 .* integral(sigma,sigma,u,Gxy,u);
Sr = -1/(2*pi) .* regularize(sigma,sigma,u,'[log(r)]',u);
S  = S + Sr;
toc

% Double layer
tic
D  = 1i/4 .* integral(sigma,sigma,u,dyGxy,ntimes(u));
Dr = -1/(2*pi) .* regularize(sigma,sigma,u,'grady[log(r)]',ntimes(u));
D  = D + Dr;
toc

% Radiation with integral representation
sol = S * (Id\lambda) - D * (Id\mu) - 0.5*mu;

% Analytic solution : Gk(X,X0) exterieur et 0 interieur
ref = - mu;
norm(ref-sol)/norm(ref)
norm(ref-sol,'inf')/norm(ref,'inf')

disp('~~> Michto gypsilab !')


% Exact solution of Riemann problem

% Copyright 2001 P. Wesseling
% This program and its subprograms may be freely used, modified and distributed
% under the GNU General Public License: http://www.gnu.org/copyleft/gpl.html

% Theory is given in Section 10.2 of
% 	Principles of Computational Fluid Dynamics, by P. Wesseling
% 	Springer-Verlag, Berlin etc., 2001. ISBN 3-540-67853-0
% See http://dutita0.twi.tudelft.nl/nw/users/wesseling/

% This program generates Figs. 10.2 -- 10.4 in the book

% Functions called: f, problem_specification

global  PRL  CRL MACHLEFT  gamma  pleft  pright  rholeft  rhoright  uleft...
	uright  tend  lambda		% lambda = dt/dx

problem_specification

gamma = 1.4; gammab = 1/(gamma - 1); gam1 = gamma-1;

% Assumed structure of exact solution
%
%    \         /      |con |       |s|
%     \   f   /       |tact|       |h|
% left \  a  /  state |disc| state |o| right
% state \ n /    2    |cont|   3   |c| state
%   1    \ /          |tinu|       |k|   4
%         |           |ity |       | |

PRL = pright/pleft;
cright = sqrt(gamma*pright/rhoright); cleft = sqrt(gamma*pleft/rholeft);
CRL = cright/cleft;
MACHLEFT = (uleft - uright)/cleft;

p34 = fzero('f',3);		% p34 = p3/p4
p3 = p34*pright; 	alpha = (gamma+1)/(gamma-1);
rho3 = rhoright*(1+alpha*p34)/(alpha+p34);
rho2 = rholeft*(p34*pright/pleft)^(1/gamma);
u2 = uleft-uright+(2/(gamma-1))*cleft*...
	(1-(p34*pright/pleft)^((gamma-1)/(2*gamma)));
c2 = sqrt(gamma*p3/rho2);
spos = 0.5 + ...		% Shock position
	tend*cright*sqrt((gamma-1)/(2*gamma) + (gamma+1)/(2*gamma)*p34)+...
	tend*uright;

conpos = 0.5 + u2*tend + tend*uright;	% Position of contact discontinuity
pos1 = 0.5 + (uleft - cleft)*tend;	% Start of expansion fan
pos2 = 0.5 + (u2+uright-c2)*tend;	% End of expansion fan
xx = 0:0.002:1;
pexact = zeros(size(xx)); uexact= zeros(size(xx)); rhoexact = zeros(size(xx));
machexact = zeros(size(xx));  cexact = zeros(size(xx));
for i = 1:length(xx)
  if xx(i) <= pos1
    pexact(i) = pleft;    rhoexact(i) = rholeft;
    uexact(i) = uleft;    cexact(i)   = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  elseif xx(i) <= pos2
    pexact(i) = pleft*(1+(pos1-xx(i))/(cleft*alpha*tend))^(2*gamma/(gamma-1));
    rhoexact(i) = rholeft*(1+(pos1-xx(i))/(cleft*alpha*tend))^(2/(gamma-1));
    uexact(i) = uleft + (2/(gamma+1))*(xx(i)-pos1)/tend;
    cexact(i) = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  elseif xx(i) <= conpos
    pexact(i) = p3;    	      rhoexact(i) = rho2;
    uexact(i) = u2+uright;    cexact(i)   = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  elseif xx(i) <= spos
    pexact(i) = p3;    rhoexact(i) = rho3;    uexact(i) = u2+uright;
    cexact(i) = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  else
    pexact(i) = pright;    rhoexact(i) = rhoright;
    uexact(i) = uright;    cexact(i)   = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  end
end
entroexact = log(pexact./rhoexact.^gamma);

figure(1), clf
subplot(2,3,1), hold on, title('DENSITY','fontsize',12),    plot(xx,rhoexact)
subplot(2,3,2), hold on, title('VELOCITY','fontsize',12),   plot(xx,uexact)
subplot(2,3,3), hold on, title('PRESSURE','fontsize',12),   plot(xx,pexact)
subplot(2,3,4), hold on, title('MACHNUMBER','fontsize',12), plot(xx,machexact)
subplot(2,3,5), hold on, title('ENTROPY','fontsize',12),    plot(xx,entroexact)


function [yi, ypi, yppi] = chermite(x, y, yp, xi, c)

% CHERMITE 1-D piecewise cubic Hermite spline
%    CHERMITE(X,Y,YP,XI,C) interpolates to find YI, the values of the
%    underlying function Y at the points in the array XI, using
%    piecewise cubic Hermite splines.  X and Y must be vectors
%    of length N.
%
%    C specifies how tangents are calculated when YP is not specified.
%    C can be:
%       0 : Finite difference (default)
%       1 : Catmull-Rom spline
%       2 : Monotone interpolation
%       3 : Monotone with Lam harmonic mean
%
%    [YI,YPI,YPPI] = CHERMITE() also returns the interpolated
%    quadratic derivative and linear second derivative of the
%    underlying function Y at points XI.

% Joe Henning - Fall 2011

if (nargin < 5)
   c = 0;
end

if (~isempty(yp))
   c = -1;
end

n = length(x);

if (c == 0)
   % precompute finite difference derivative
   yp = lfindiff (x, y);
elseif (c == 3)
   % precompute harmonic mean
   yp = zeros(1,n);
   d = zeros(1,n-1);

   for i = 1:n-1
      h = x(i+1) - x(i);
      dy = y(i+1) - y(i);
      d(i) = dy/h;
   end

   for i = 2:n-1
      if (d(i-1)*d(i) > 0)
         yp(i) = 2*d(i-1)*d(i)/(d(i-1) + d(i));
      else
         yp(i) = 0;
      end
   end

   if (d(1)*(2*d(1)-yp(2)) > 0)
      yp(1) = 2*d(1) - yp(2);
   else
      yp(1) = 0;
   end

   if (d(n-1)*(2*d(n-1)-yp(n-1)) > 0)
      yp(n) = 2*d(n-1) - yp(n-1);
   else
      yp(n) = 0;
   end
end

for i = 1:length(xi)
   % Find the right place in the table by means of a bisection.
   klo = 1;
   khi = n;
   while (khi-klo > 1)
      k = fix((khi+klo)/2.0);
      if (x(k) > xi(i))
         khi = k;
      else
         klo = k;
      end
   end

   h = x(khi) - x(klo);
   if (h == 0.0)
      fprintf('??? Bad x input to chermite ==> x values must be distinct\n');
      yi(i) = NaN;
      ypi(i) = NaN;
      yppi(i) = NaN;
      continue;
   end

   if (c == -1)
      a = yp(klo);
      b = yp(khi);
   elseif (c == 0)
      % Finite difference
      a = yp(klo);
      b = yp(khi);
   elseif (c == 1)
      % Catmull-Rom spline
      if (klo == 1)
         a = (y(khi) - y(klo))/h;
         b = (y(khi+1) - y(klo))/(x(khi+1) - x(klo));
      elseif (khi == n)
         a = (y(khi) - y(klo-1))/(x(khi) - x(klo-1));
         b = (y(khi) - y(klo))/h;
      else
         a = (y(khi) - y(klo-1))/(x(khi) - x(klo-1));
         b = (y(khi+1) - y(klo))/(x(khi+1) - x(klo));
      end
   else
      % Monotone interpolation
      if (c == 2)
         if (klo == 1)
            a = (y(khi) - y(klo))/h;
            b = ( (y(khi) - y(klo))/h + (y(khi+1) - y(khi))/(x(khi+1) - x(khi)) )/2;
         elseif (khi == n)
            a = ( (y(klo) - y(klo-1))/(x(klo) - x(klo-1)) + (y(khi) - y(klo))/h )/2;
            b = (y(khi) - y(klo))/h;
         else
            a = ( (y(klo) - y(klo-1))/(x(klo) - x(klo-1)) + (y(khi) - y(klo))/h )/2;
            b = ( (y(khi) - y(klo))/h + (y(khi+1) - y(khi))/(x(khi+1) - x(khi)) )/2;
         end
      else
         % Harmonic mean
         a = yp(klo);
         b = yp(khi);
      end

      if ( y(khi) == y(klo) )
         a = 0;
         b = 0;
      else
         alpha = a/( (y(khi) - y(klo))/h );
         beta = b/( (y(khi) - y(klo))/h );
         if (alpha < 0 || beta < 0)
            a = 0;
            b = 0;
         else
            if ( (alpha*alpha + beta*beta) > 9 )
               tau = 3/sqrt(alpha*alpha + beta*beta);
               a = tau*alpha*(y(khi) - y(klo))/h;
               b = tau*beta*(y(khi) - y(klo))/h;
            end
         end
      end
   end

   % Evaluate cubic Hermite polynomial
   t = (xi(i) - x(klo))/h;
   t2 = t*t;
   t3 = t2*t;
   h2 = h*h;
   h00 = 2*t3 - 3*t2 + 1;
   h10 = t3 - 2*t2 + t;
   h01 = -2*t3 + 3*t2;
   h11 = t3 - t2;

   yi(i) = h00*y(klo) + h10*h*a + h01*y(khi) + h11*h*b;

   % Differentiate to find the second-order interpolant
   h00 = 6*t2 - 6*t;
   h10 = 3*t2 - 4*t + 1;
   h01 = -h00;
   h11 = 3*t2 - 2*t;

   ypi(i) = h00*y(klo)/h + h10*a + h01*y(khi)/h + h11*b;

   % Differentiate to find the first-order interpolant
   h00 = 12*t - 6;
   h10 = 6*t - 4;
   h01 = -h00;
   h11 = 6*t - 2;

   yppi(i) = h00*y(klo)/h2 + h10*a/h + h01*y(khi)/h2 + h11*b/h;
end

% 3-pt finite difference
function [yp] = lfindiff(x, y)
% Finite Difference Formulae for Unequal Sub-Intervals Using Lagrange's Interpolation Formula
% Singh, Ashok K. and B. S. Bhadauria
% Int. Journal of Math. Analysis, Vol. 3, 2009, no. 17, 815-827

n = length(x);

if (n ~= length(y))
   fprintf('Error, len(x) != len(y), returning\n');
   yp = NaN;
   return;
end

for k = 1:n
   if (k == 1)
      h1 = x(k+1) - x(k);
      h2 = x(k+2) - x(k+1);
      yp(k) = -(2*h1+h2)/(h1*(h1+h2))*y(k) + (h1+h2)/(h1*h2)*y(k+1) - h1/(h2*(h1+h2))*y(k+2);
   elseif (k == n)
      h1 = x(k-1) - x(k-2);
      h2 = x(k) - x(k-1);
      yp(k) = h2/(h1*(h1+h2))*y(k-2) - (h1+h2)/(h1*h2)*y(k-1) + (h1+2*h2)/(h2*(h1+h2))*y(k);
   else
      h1 = x(k) - x(k-1);
      h2 = x(k+1) - x(k);
      yp(k) = -h2/(h1*(h1+h2))*y(k-1) - (h1-h2)/(h1*h2)*y(k) + h1/(h2*(h1+h2))*y(k+1);
   end
end


function q = vgg_quat_from_rotation_matrix( R )
% vgg_quat_from_rotation_matrix Generates quaternion from rotation matrix
%            q = vgg_quat_from_rotation_matrix(R)

q = [	(1 + R(1,1) + R(2,2) + R(3,3))
	(1 + R(1,1) - R(2,2) - R(3,3))
	(1 - R(1,1) + R(2,2) - R(3,3))
	(1 - R(1,1) - R(2,2) + R(3,3)) ];

if ~issym(q)
  % Pivot to avoid division by small numbers
  [b I] = max(abs(q));
else
  % For symbolic quats, just make sure we're nonzero
  for k=1:4
    if q(k) ~= 0
      I = k;
      break
    end
  end
end

q(I) = sqrt(q(I)) / 2 ;

if I == 1
	q(2) = (R(3,2) - R(2,3)) / (4*q(I));
	q(3) = (R(1,3) - R(3,1)) / (4*q(I));
	q(4) = (R(2,1) - R(1,2)) / (4*q(I));
elseif I==2
	q(1) = (R(3,2) - R(2,3)) / (4*q(I));
	q(3) = (R(2,1) + R(1,2)) / (4*q(I));
	q(4) = (R(1,3) + R(3,1)) / (4*q(I));
elseif I==3
	q(1) = (R(1,3) - R(3,1)) / (4*q(I));
	q(2) = (R(2,1) + R(1,2)) / (4*q(I));
	q(4) = (R(3,2) + R(2,3)) / (4*q(I));
elseif I==4
	q(1) = (R(2,1) - R(1,2)) / (4*q(I));
	q(2) = (R(1,3) + R(3,1)) / (4*q(I));
	q(3) = (R(3,2) + R(2,3)) / (4*q(I));
end


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

function sphcoord = polarcoord(v)

%%%%% Cartesian to spherical coordinates
%%%%% v = n x 3 array of pts
%%%%% output is n x 3 array of spherical coordinates
%%%%% [r theta phi]

r = sqrt(v(:,1).^2 + v(:,2).^2 + v(:,3).^2);
ind1 = find(r == 0);
ind2 = find(r ~= 0);

if(size(ind1,2) > 0)
theta(ind1)=0; phi(ind1)=0;
end

if(size(ind2,2) > 0)
    theta(ind2) = acos(v(ind2,3)./r(ind2));
    phi(ind2) = atan2(v(ind2,2),v(ind2,1));
end

sphcoord = [r theta' phi'];
end


clear all; close all;

dt = 0.02;
sim_t = 20;
x0 = [0; 20; 100];

%% Parameters are from
% Aaron Ames et al. Control Barrier Function based Quadratic Programs
% with Application to Adaptive Cruise Control, CDC 2014, Table 1.

params.v0 = 14;
params.vd = 24;
params.m  = 1650;
params.g = 9.81;
params.f0 = 0.1;
params.f1 = 5;
params.f2 = 0.25;
params.ca = 0.3;
params.cd = 0.3;
params.T = 1.8;

params.u_max = params.ca * params.m * params.g;
params.u_min  = -params.cd * params.m * params.g;

params.clf.rate = 5;
params.cbf.rate = 5;

params.weight.input = 2/params.m^2;
params.weight.slack = 2e-2;

%%
accSys = ACC(params);

odeFun = @accSys.dynamics;
controller = @accSys.ctrlCbfClfQp;
odeSolver = @ode45;

total_k = ceil(sim_t / dt);
x = x0;
t = 0;
% initialize traces.
xs = zeros(total_k, 3);
ts = zeros(total_k, 1);
us = zeros(total_k-1, 1);
slacks = zeros(total_k-1, 1);
hs = zeros(total_k-1, 1);
Vs = zeros(total_k-1, 1);
xs(1, :) = x0';
ts(1) = t;
for k = 1:total_k-1
    t
    Fr = accSys.getFr(x);
    % Determine control input.
    [u, slack, h, V] = controller(x, Fr);
    us(k, :) = u';
    slacks(k, :) = slack;
    hs(k) = h;
    Vs(k) = V;

    % Run one time step propagation.
    [ts_temp, xs_temp] = odeSolver(@(t, s) odeFun(t, s, u), [t t+dt], x);
    x = xs_temp(end, :)';

    xs(k+1, :) = x';
    ts(k+1) = ts_temp(end);
    t = t + dt;
end

plot_results(ts, xs, us, slacks, hs, Vs, params)

function plot_results(ts, xs, us, slacks, hs, Vs, params)
    fig_sz = [10 15];
    plot_pos = [0 0 10 15];
    yellow = [0.998, 0.875, 0.529];
    blue = [0.106, 0.588, 0.953];
    navy = [0.063, 0.075, 0.227];
    magenta = [0.937, 0.004, 0.584];
    orange = [0.965, 0.529, 0.255];
    grey = 0.01 *[19.6, 18.8, 19.2];

    figure(1);
    subplot(6,1,1);
    p = plot(ts, xs(:, 2));
    p.Color = blue;
    p.LineWidth = 1.5;
    hold on;
    plot(ts, params.vd*ones(size(ts, 1), 1), 'k--');
    ylabel("v (m/s)");
    title("State - Velocity");
    set(gca,'FontSize',14);
    grid on;

    subplot(6,1,2);
    p = plot(ts, xs(:, 3));
    p.Color = magenta;
    p.LineWidth = 1.5;
    ylabel("z (m)");
    title("State - Distance to lead vehicle");
    set(gca, 'FontSize', 14);
    grid on;

    subplot(6,1,3);
    p = plot(ts(1:end-1), us); hold on;
    p.Color = orange;
    p.LineWidth = 1.5;
    plot(ts(1:end-1), params.u_max*ones(size(ts, 1)-1, 1), 'k--');
    plot(ts(1:end-1), params.u_min*ones(size(ts, 1)-1, 1), 'k--');
    ylabel("u(N)");
    title("Control Input - Wheel Force");
    set(gca, 'FontSize', 14);
    grid on;

    subplot(6,1,4);
    p = plot(ts(1:end-1), slacks); hold on;
    p.Color = magenta;
    p.LineWidth = 1.5;
    ylabel("slack");
    title("Slack variable");
    set(gca, 'FontSize', 14);
    grid on;

    subplot(6,1,5);
    p = plot(ts(1:end-1), hs);
    p.Color = navy;
    p.LineWidth = 1.5;
    ylabel("CBF (h(x))");
    title("CBF");
    set(gca, 'FontSize', 14);
        grid on;

    subplot(6,1,6);
    set(gca, 'FontSize', 14);
    p = plot(ts(1:end-1), Vs);
    p.Color = navy;
    p.LineWidth = 1.5;
    xlabel("t(s)");
    ylabel("CLF (V(x))");
    title("CLF");
    set(gca, 'FontSize', 14);
    grid on;

    set(gcf, 'PaperSize', fig_sz);
    set(gcf, 'PaperPosition', plot_pos);
end


function [x,stat] = libqp_gsmo(H,f,a,b,LB,UB,x0,opt)
% LIBQP_GSMO Generalized SMO algorithm.
%
% Synopsis:
%  [x,stat] = libqp_gsmo(H,f,a,b,LB,UB)
%  [x,stat] = libqp_gsmo(H,f,a,b,LB,UB,x0)
%  [x,stat] = libqp_gsmo(H,f,a,b,LB,UB,[],opt)
%  [x,stat] = libqp_gsmo(H,f,a,b,LB,UB,x0,opt)
%
% Description:
%  This function implements the Generalized SMO algorithm which solves
%  the following convex quadratic programming task:
%
%   min 0.5*x'*H*x + f'*x
%    x
%
%   subject to   a'*x = b
%                LB(i) <= x(i) <= UB(i)   for all i=1:n
%
% Reference:
%  S.-S. Keerthi, E.G.Gilbert. Convergence of a Generalized SMO Algorithm for SVM
%   Classifier Design. Technical Report CD-00-01, Control Division, Dept. of Mechanical
%   and Production Engineering, National University of Singapore, 2000.
%   http://citeseer.ist.psu.edu/keerthi00convergence.html
%
% Input:
%  H [n x n] Symmetric positive semi-definite matrix.
%  f [n x 1] Vector.
%  a [n x 1] Vector which must not contain zero entries.
%  b [1 x 1] Scalar.
%  LB [n x 1] Lower bound; -inf is allowed.
%  UB [n x 1] Upper bound; inf is allowed.
%
% Optional inputs:
%  x0 [n x 1] Initial solution.
%
%  options [struct]
%    .TolKKT [1 x 1] Determines relaxed KKT conditions (default TolKKT=0.001);
%       it correspondes to $\tau$ in Keerthi's paper.
%    .verb [1 x 1] if > 0 then prints info every verb-th iterations (default 0)
%    .MaxIter [1 x 1] Maximal number of iterations (default inf).
%
% Output:
%  x [n x 1] Solution vector.
%  stat [struct]
%   .QP [1x1] Primal objective value.
%   .exitflag [1 x 1] Indicates which stopping condition was used:
%      nIter >= MaxIter                  ->  exitflag = 0
%      relaxed KKT conditions satisfied  ->  exitflag = 4
%   .nIter [1x1] Number of iterations.
%
% Example:
%  n=50; X = rand(n,n); H = X'*X; f = -10*rand(n,1);
%  a=rand(1,n)+0.1; b=rand; tmp = rand(n,1); LB = tmp-1; UB = tmp+1;
%
%  tic; [x1,stat1] = libqp_gsmo(H,f,a,b,LB,UB); fval1=stat1.QP, toc
%  tic; [x2,fval2] = quadprog(H,f,[],[],a,b,LB,UB); fval2, toc
%

% Copyright (C) 2006-2008 Vojtech Franc, xfrancv@cmp.felk.cvut.cz
% Center for Machine Perception, CTU FEL Prague

% options
if nargin < 6, error('At least six input arguments are required.'); end
if nargin < 7 || isempty(x0),
  % create feasible solution x0 if not given
  x0 = zeros(length(f),1);
  xa = 0; i = 0;
  while x0'*a(:) ~= b,
    i = i + 1;
    if i > length(a),
      error('No feasible solution exists.');
    end
    x0(i) = min(UB(i),max(LB(i),(b-xa)/a(i)));
    xa = xa + x0(i)*a(i);
  end
end

if nargin < 8, opt = []; end
if ~isfield(opt,'TolKKT'), opt.TolKKT = 0.001; end
if ~isfield(opt,'MaxIter'), opt.MaxIter = inf; end
if ~isfield(opt,'verb'), opt.verb = 0; end

[x,stat.QP,stat.exitflag,stat.nIter] = ...
    libqp_gsmo_mex(H,f,a,b,LB,UB,x0,opt.MaxIter,opt.TolKKT,opt.verb);

return;
% EOF


function [ x, w ] = rule06 ( n )

%*****************************************************************************80
%
%% RULE06 returns the rule of degree 6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer N, the number of nodes.
%
%    Output, real X(2,N), the coordinates of the nodes.
%
%    Output, real W(N), the weights.
%
  xs = [ ...
    -.7347550761673839,0.8662152988634034, ...
    0.1596873653424614,-.8905137714296896, ...
    -.1469707846748791,0.9240009259977663, ...
    -.8463324986375500,-.4086308482879689, ...
    0.5175294652720337,0.4801002492857063 ];
  ys = [ ...
    0.8933891941643415,-.7037359670513631, ...
    -.9085856749287847,0.1644347368502312, ...
    0.5352177835541986,0.4879643750888035, ...
    -.8394767448218339,-.4262330870004397, ...
    0.9176357850707917,-.1009764561823168 ];
  ws = [ ...
    0.1541850714382379,0.1900556513689156, ...
    0.2246942645703077,0.2465847648329768, ...
    0.5062382287542438,0.1829226437278864, ...
    0.1373586623279704,0.4754388545735908, ...
    0.1856913242244974,0.5252576589275637 ];

  x(1,1:n) = xs(1:n);
  x(2,1:n) = ys(1:n);
  w(1:n) = ws(1:n);

  return
end


function y=modulo(x,N);
%MODULO	Congruence of a vector.
%	Y=MODULO(X,N) gives the congruence of each element of the
%	vector X modulo N. These values are strictly positive and
%	lower equal than N.
%
%	See also REM.

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

if isreal(x)
  y=mod(x,N);
  idx=find(y==0);
  y(idx)=N;
else
  y=mod(real(x),N)+i*mod(imag(x),N);
end;


function sphere_triangle_test01 ( )

%*****************************************************************************80
%
%% SPHERE_TRIANGLE_TEST01 tests SPHERE01_TRIANGLE_QUAD_01, 02, 03.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2014
%
%  Author:
%
%    John Burkardt
%
  e_test = [ ...
    0, 0, 0; ...
    1, 0, 0; ...
    0, 1, 0; ...
    0, 0, 1; ...
    2, 0, 0; ...
    0, 2, 2; ...
    2, 2, 2; ...
    0, 2, 4; ...
    0, 0, 6; ...
    1, 2, 4; ...
    2, 4, 2; ...
    6, 2, 0; ...
    0, 0, 8; ...
    6, 0, 4; ...
    4, 6, 2; ...
    2, 4, 8; ...
   16, 0, 0 ]';

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  Approximate an integral on a random spherical triangle.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  QUAD_01 uses centroids of spherical triangles.\n' );
  fprintf ( 1, '  QUAD_02 uses vertices of spherical triangles.\n' );
  fprintf ( 1, '  QUAD_03 uses midsides of spherical triangles.\n' );
%
%  Choose three points at random to define a spherical triangle.
%
  [ v1, seed ] = sphere01_sample ( 1, seed );
  [ v2, seed ] = sphere01_sample ( 1, seed );
  [ v3, seed ] = sphere01_sample ( 1, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Vertices of random spherical triangle:\n' );
  fprintf ( 1, '\n' );
  r8vec_transpose_print ( 3, v1, '  V1:' );
  r8vec_transpose_print ( 3, v2, '  V2:' );
  r8vec_transpose_print ( 3, v3, '  V3:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  QUAD_01      QUAD_02      QUAD_03\n' );

  for j = 1 : 17

    e(1:3) = e_test(1:3,j);

    e = polyterm_exponent ( 'SET', e );

    polyterm_exponent ( 'PRINT', e );

    result_01 = sphere01_triangle_quad_01 ( v1, v2, v3, @polyterm_value_3d );

    result_02 = sphere01_triangle_quad_02 ( v1, v2, v3, @polyterm_value_3d );

    result_03 = sphere01_triangle_quad_03 ( v1, v2, v3, @polyterm_value_3d );

    fprintf ( 1, '  %14.6g  %14.6g  %14.6g\n', result_01, result_02, result_03 );

  end

  return
end


% MAIN_passiveSimulate.m
%
% This script runs a passive simulation of the acrobot, to do a sanity
% check on the dynamics and plotting routines
%
% Things to try:
%
% 1) Adjust the parameters and states and see if you can guess what will
% happen when you run the simulation. What if one mass or length is much
% bigger than the other? Can you make the system behave like a single
% pendulum?
%
% 2) Type:      >> help animate
% to see the keyboard commands for controlling the animation. You can use
% then to see the system in slow motion, pause, or go back in time, to name
% a few possiblities. This is useful for understanding what the system is
% doing.
%

clc; clear;

% Physical parameters
p.m1 = 10;
p.m2 = 20;
p.g = 9;
p.l1 = 3;
p.l2 = 5;

% Initial state:
q1 = (pi/180)*120;
q2 = (pi/180)*50;
dq1 = 0;
dq2 = 0;
z0 = [q1;q2;dq1;dq2];  %Pack up initial state

tSpan = [0,8];  %time span for the simulation
dynFun = @(t,z)( acrobotDynamics(z,0,p) );  %passive dynamics function

% Run simulation:
sol = ode45(dynFun,tSpan,z0);
t = linspace(tSpan(1),tSpan(2),500);
z = deval(sol,t);
u = zeros(size(t));

% Animate the results:
A.plotFunc = @(t,z)( drawAcrobot(t,z,p) );
A.speed = 1.0;
A.figNum = 101;
A.verbose = true;
animate(t,z,A)

% Plot the results:
figure(1337); clf; plotAcrobot(t,z,u,p)


function Xs = DVV_surrogate(X, Ns)
% Generates surrogate data for real and complex signals
%
% Surrogate data generation using iterated amplitude adjusted fourier
% transform (iAAFT) method for real-valued series, and complex iAAFT method
% for complex series.
%
%
% USAGE:    Xs = surrogate(X, Ns)
%
% INPUTS:
% X:        Input time series (can be real-valued or complex)
% Ns:       No. of surrogates to be generated
%
% OUTPUTS:
% Xs:   Generated surrogates
%
%
%   A Delay Vector Variance (DVV) toolbox for MATLAB
%   (c) Copyright Danilo P. Mandic 2008
%   http://www.commsp.ee.ic.ac.uk/~mandic/dvv.htm
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Default Parameters
if (nargin<1)
    error('Not enough Input arguments');
end
if (nargin<2)
    Ns = 25;
end

% Initial conditions and parameter initializations
iter = 0;
max_it = 1000;               % Maximum iterations
error_threshold = 1e-5;
MSE_start = 100;
MSE = 1000;

% Makes input vector X a column vector
if (size(X,2) > size(X,1))
    X = X';
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Implements univariate iAAFT algorithm for real-valued signals
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (isreal(X))

    Xs = zeros(length(X),Ns);

    % Stores the Amplitude vector of the original time series
    X_amp = abs(fft(X));

    % Stores the sorted version(ascending) of the original time series
    X_sorted = sort(X);

    % Iteration process for multiple surrogate time series generation
    for a = 1:Ns

        % Random permutation of the original time series
        X_random = X(randperm(length(X)));

        % Initializations
        r_prev = X_random;
        MSE_prev = MSE_start;

        % Iterate untill convergence condition is met or max iterations are reached
        while (abs(MSE-MSE_prev) > error_threshold && iter < max_it)

            MSE_prev = MSE;

            % Amplitude spectrum matching
            ang_r_prev = angle(fft(r_prev));
            s = ifft(X_amp .* exp(ang_r_prev.*sqrt(-1)));

            % Rank ordering in order to scale to original signal distribution
            [s_sort, Ind] = sort(s);
            r(Ind,:) = X_sorted;

            % Convergence Metric calculation
            MSE = mean(abs(X_amp - abs(fft(r))));

            r_prev = r;
            iter = iter+1;
        end

        %         iter
        Xs(:,a) = r;
        iter = 0;
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Implements Complex iAAFT Algorithm for complex signals
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
else

    % Initial conditions
    Xs = zeros(length(X),Ns);
    r_real = zeros(length(X),1);
    r_imag = zeros(length(X),1);
    r = zeros(length(X),1);
    r_prev = zeros(length(X),1);

    % Stores the Amplitude vector of the original time series
    X_amp = abs(fft(X));

    % Stores the sorted version(modulus, ascending), of the original time series
    C_sorted = sort(abs(X));

    % Sorted versions(real and imaginary), of original time series
    C_real_sorted = sort(real(X));
    C_imag_sorted = sort(imag(X));

    % Iteration process for multiple surrogate time series generation
    for a = 1:Ns

        % Random permutation of the original time series
        r_prev = X(randperm(length(X)));
        MSE_prev = MSE_start;

         % Iterate untill convergence condition is met or max iterations are reached
        while( abs(MSE-MSE_prev) > error_threshold && iter < max_it)

            MSE_prev = MSE;

            % Amplitude spectrum matching
            ang_r_prev = angle(fft(r_prev));
            s = ifft(X_amp .* exp(ang_r_prev.*sqrt(-1)));

            % Rank ordering real and imaginary parts of s to match that of original signal
            [temp , Ind_real] = sort(real(s));
            [temp , Ind_imag] = sort(imag(s));
            r_real(Ind_real) = C_real_sorted;
            r_imag(Ind_imag) = C_imag_sorted;

            r = complex(r_real, r_imag);

            % Rank ordering in order to scale to original signal distribution
            [temp, Ind_abs] = sort(abs(r));
            AUX1 = abs(temp);
            r(Ind_abs) = r(Ind_abs).*(abs(C_sorted)./(AUX1+(AUX1==0)));

            % Convergence Metric
            MSE = mean(abs(X_amp - abs(fft(r))));
            r_prev = r;
            iter = iter + 1;
        end

        %         iter
        Xs(:,a) = r;
        iter = 0;
    end

end


function M = spheresymmetricfactory(n)
% Returns a manifold struct to optimize over unit-norm symmetric matrices.
%
% function M = spheresymmetricfactory(n)
%
% Manifold of n-by-n real symmetric matrices of unit Frobenius norm.
% The metric is such that the sphere is a Riemannian submanifold of the
% space of nxn symmetric matrices with the usual trace inner product, i.e.,
% the usual metric <A, B> = trace(A'*B).
%
% See also: spherefactory obliquefactory spherecomplexfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, April 17, 2015.
% Contributors:
% Change log:

    M.name = @() sprintf('Sphere of symmetric matrices of size %d', n);

    M.dim = @() n*(n+1)/2 - 1;

    M.inner = @(x, d1, d2) d1(:).'*d2(:);

    M.norm = @(x, d) norm(d, 'fro');

    M.dist = @(x, y) real(acos(x(:).'*y(:)));

    M.typicaldist = @() pi;

    M.proj = @proj;
    function xdot = proj(x, d)
        d = (d+d.')/2;
        xdot = d - x*(x(:).'*d(:));
    end

    M.tangent = @proj;

    % For Riemannian submanifolds, converting a Euclidean gradient into a
    % Riemannian gradient amounts to an orthogonal projection.
	M.egrad2rgrad = @proj;

	M.ehess2rhess = @ehess2rhess;
	function rhess = ehess2rhess(x, egrad, ehess, u)
        % these are not explicitly required, given the use.
        % egrad = (egrad + egrad.')/2;
        % ehess = (ehess + ehess.')/2;
        rhess = proj(x, ehess) - (x(:)'*egrad(:))*u;
	end

    M.exp = @exponential;

    M.retr = @retraction;

    M.log = @logarithm;
    function v = logarithm(x1, x2)
        v = proj(x1, x2 - x1);
        di = M.dist(x1, x2);
        % If the two points are "far apart", correct the norm.
        if di > 1e-6
            nv = norm(v, 'fro');
            v = v * (di / nv);
        end
    end

    M.hash = @(x) ['z' hashmd5(x(:))];

    M.rand = @() random(n);

    M.randvec = @(x) randomvec(n, x);

    M.lincomb = @matrixlincomb;

    M.zerovec = @(x) zeros(n);

    M.transp = @(x1, x2, d) proj(x2, d);

    M.pairmean = @pairmean;
    function y = pairmean(x1, x2)
        y = x1+x2;
        y = y / norm(y, 'fro');
    end

    % TODO : check isometry and fix.
    M.vec = @(x, u_mat) u_mat(:);
    M.mat = @(x, u_vec) reshape(u_vec, [n, m]);
    M.vecmatareisometries = @() false;

end

% Exponential on the sphere
function y = exponential(x, d, t)

    if nargin == 2
        t = 1;
    end

    td = t*d;

    nrm_td = norm(td, 'fro');

    if nrm_td > 4.5e-8
        y = x*cos(nrm_td) + td*(sin(nrm_td)/nrm_td);
    else
        % If the step is too small to accurately evaluate sin(x)/x,
        % then sin(x)/x is almost indistinguishable from 1.
        y = x + td;
        y = y / norm(y, 'fro');
    end

end

% Retraction on the sphere
function y = retraction(x, d, t)

    if nargin == 2
        t = 1;
    end

    y = x + t*d;
    y = y / norm(y, 'fro');

end

% Uniform random sampling on the sphere.
function x = random(n)

    x = randn(n);
    x = (x + x.')/2;
    x = x/norm(x, 'fro');

end

% Random normalized tangent vector at x.
function d = randomvec(n, x)

    d = randn(n);
    d = (d + d.')/2;
    d = d - x*(x(:).'*d(:));
    d = d / norm(d, 'fro');

end


function odf = mix2
% mix2 sample ODF

CS = crystalSymmetry('cubic');
SS = specimenSymmetry('222');

psi = SO3vonMisesFisherKernel('halfwidth',17*degree);
ori1 = orientation.byEuler(54.736*degree, 45.0*degree, 0.0*degree,CS,SS);
ori2 = orientation.byEuler(62.968*degree, 57.689*degree, 71.565*degree,CS,SS);
ori3 = orientation.byEuler(50.768*degree, 65.905*degree, 63.435*degree,CS,SS);

odf = 0.3095*uniformODF(CS,SS) + 0.315*unimodalODF(ori1,psi) + ...
  0.315*unimodalODF(ori2,psi) + 0.0605*unimodalODF(ori3,psi);

% 3,7
% MIX2
% 3,3,0
% 0.3095
% 54.736,45.0,0.0
% 1,17.0,0.315,2
% 62.968,57.689,71.565
% 1,17.0,0.315,2
% 50.768,65.905,63.435
% 1,17.0,0.0605,1


function [out] = interflow_9(S,p1,p2,p3,dt)
%interflow_9

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Non-linear interflow if storage exceeds a threshold
% Constraints:  f <= S-p2
%               S-p2 >= 0     prevents numerical issues with complex numbers
% @(Inputs):    p1   - time coefficient [d-1]
%               p2   - storage threshold for flow generation [mm]
%               p3   - exponential scaling parameter [-]
%               S    - current storage [mm]
%               dt   - time step size [d]

out = min(max((S-p2)/dt,0),(p1.*max(S-p2,0)).^p3);

end


%% Transient diffusion equation
%% PDE and boundary conditions
% The transient diffusion equation reads
%
% $$\alpha\frac{\partial c}{\partial t}+\nabla.\left(-D\nabla c\right)=0,$$
%
% where $c$ is the independent variable (concentration, temperature, etc)
% , $D$ is the diffusion coefficient, and $\alpha$ is a constant.
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc;

%% Define the domain and create a mesh structure
L = 10;  % domain length
Nx = 20; % number of cells
Ntetta = 21;
m = createMeshRadial2D(Nx,Ntetta, L,2*pi);
%% Create the boundary condition structure
BC = createBC(m); % all Neumann boundary condition structure
m.cellcenters.x=m.cellcenters.x+1;
m.facecenters.x=m.facecenters.x+1;
BC.left.a(1:3:20) = 0; BC.left.b(1:3:20)=1; BC.left.c(1:3:20)=10; % left boundary
BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=0; % right boundary
% BC.right.a(1:floor(Ntetta/2)) = 0; BC.right.b(1:floor(Ntetta/2))=1; BC.right.c(1:floor(Ntetta/2))=3; % right boundary
BC.top.periodic=1;
% BC.top.a(:) = 0; BC.top.b(:)=1; BC.top.c(:)=0; % top boundary
% BC.bottom.a(:) = 0; BC.bottom.b(:)=1; BC.bottom.c(:)=0; % bottom boundary
%% define the transfer coeffs
D_val = 0.01;
D = createCellVariable(m, D_val);
alfa = createCellVariable(m, 1);
u_val=1;
u = createFaceVariable(m, [u_val, 0]);
%% define initial values
c_init = 0.1;
c_old = createCellVariable(m, c_init,BC); % initial values
c = c_old; % assign the old value of the cells to the current values
%% loop
dt = 0.5; % time step
final_t = 7;
Dave = harmonicMean(D);
Mdiff = diffusionTerm(Dave);
[Mbc, RHSbc] = boundaryCondition(BC);
FL = fluxLimiter('Superbee');
Mconv = convectionTerm(u);
for t=dt:dt:final_t
    [M_trans, RHS_trans] = transientTerm(c, dt, alfa);
    M = M_trans+Mconv-Mdiff+Mbc;
    RHS = RHS_trans+RHSbc;
    c = solvePDE(m,M, RHS);
    c_old = c.value;
    figure(1);visualizeCells(c);shading interp; drawnow;
end


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


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


%[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 [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 [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 [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 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 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);