function value = i4_characteristic ( q )

%*****************************************************************************80
%
%% I4_CHARACTERISTIC gives the characteristic for an integer.
%
%  Discussion:
%
%    For any positive integer Q, the characteristic is:
%
%    Q, if Q is a prime;
%    P, if Q = P^N for some prime P and some integer N;
%    0, otherwise, that is, if Q is negative, 0, 1, or the product
%       of more than one distinct prime.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2005
%
%  Author:
%
%    FORTRAN77 original version by Bratley, Fox, Niederreiter.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Harald Niederreiter,
%    Algorithm 738:
%    Programs to Generate Niederreiter's Low-Discrepancy Sequences,
%    ACM Transactions on Mathematical Software,
%    Volume 20, Number 4, pages 494-495, 1994.
%
%  Parameters:
%
%    Input, integer Q, the value to be tested.
%
%    Output, integer VALUE, the characteristic of Q.
%
  if ( q <= 1 )
    value = 0;
    return
  end
%
%  If Q is not prime, then there is at least one prime factor
%  of Q no greater than SQRT(Q)+1.
%
%  A faster code would only consider prime values of I,
%  but that entails storing a table of primes and limiting the
%  size of Q.  Simplicity and flexibility for now!
%
  i_max = floor ( sqrt ( q ) ) + 1;

  for i = 2 : i_max

    if ( mod ( q, i ) == 0 )

      while ( mod ( q, i ) == 0 )
        q = q / i;
      end

      if ( q == 1 )
        value = i;
      else
        value = 0;
      end

      return

    end

  end
%
%  If no factor was found, then Q is prime.
%
  value = q;

  return
end


function [A1, B1, C1, A2, B2, C2] = sss(a, b, c)
%SSS   gives both solutions to the side-side-side problem, in radians.
%
%   SSS(a, b, c) results in NaNs for those indices where the existence
%   condition |pi - a| - |pi - b| <= |pi - c| <= |pi - a| + |pi -b| is not
%   met.
%
%   See also SSSD.

% Rody P.S. Oldenhuis
% Delft University of Technology
% oldenhuis@gmail.com
%
% Created    : 23/Feb/2009
% Last edited: 30/Nov/2012

    % first solution
    A1 = acos2( (cos(a) - cos(b).*cos(c))./(sin(b).*sin(c)), a);
    B1 = acos2( (cos(b) - cos(a).*cos(c))./(sin(a).*sin(c)), b);
    C1 = acos2( (cos(c) - cos(a).*cos(b))./(sin(a).*sin(b)), c);

    % second solution
    A2 = 2*pi - A1;
    B2 = 2*pi - B1;
    C2 = 2*pi - C1;

    % check constraints
    indices = ( ...
        (abs(pi-a) - abs(pi-b)) > abs(pi-c) | ...
        abs(pi-c) > (abs(pi-a) + abs(pi-b)) );
    A1(indices) = NaN;   B1(indices) = NaN;   C1(indices) = NaN;
    A2(indices) = NaN;   B2(indices) = NaN;   C2(indices) = NaN;

end


function proj2d = projection(data3d,param, angle_rad)

proj2d = zeros(param.nu,param.nv,'single');

[uu,vv] = meshgrid(param.us,param.vs);

[xx,yy] = meshgrid(param.xs,param.ys);

rx = xx.*cos(angle_rad) - yy.*sin(angle_rad);
ry = xx.*sin(angle_rad) + yy.*cos(angle_rad);

for iz = 1:param.nz

    data3d(:,:,iz) = interp2(xx,yy ,data3d(:,:,iz),rx,ry,'linear');

end

data3d(isnan(data3d))=0;

data3d = permute(data3d,[1 3 2]);

[xx,zz] = meshgrid(param.xs,param.zs);

for iy = 1:param.ny

    Ratio = (param.ys(iy)+param.DSO)/(param.DSD);

    pu = uu*Ratio;
    pv = vv*Ratio;

    pu = (pu - xx(1,1))/(param.dx)+1;
    pv = (pv - zz(1,1))/(param.dz)+1;

    tmp = interp2(data3d(:,:,iy),pv,pu,'linear');

    tmp(isnan(tmp))=0;

    proj2d = proj2d + tmp';
end

dist = sqrt((param.DSD)^2 + uu.^2 + vv.^2)./(param.DSD)*param.dy;

proj2d = proj2d .* dist';


function [ output_args ] = test_image_registration( input_args )
%TEST_IMAGE_REGISTRATION Summary of this function goes here
%   Detailed explanation goes here
Imoving=imread('images/lenag1.png');
Istatic=imread('images/lenag2.png');

% Register the images
[Ireg,O_trans,Spacing,M,B,F] = image_registration(Imoving,Istatic);

% Show the registration result
figure,
subplot(2,2,1), imshow(Imoving); title('moving image');
subplot(2,2,2), imshow(Istatic); title('static image');
subplot(2,2,3), imshow(Ireg); title('registerd moving image');
% Show also the static image transformed to the moving image
Ireg2=movepixels(Istatic,F);
subplot(2,2,4), imshow(Ireg2); title('registerd static image');

% Show the transformation fields
figure,
subplot(2,2,1), imshow(B(:,:,1),[]); title('Backward Transf. in x direction');
subplot(2,2,2), imshow(F(:,:,2),[]); title('Forward Transf. in x direction');
subplot(2,2,3), imshow(B(:,:,1),[]); title('Backward Transf. in y direction');
subplot(2,2,4), imshow(F(:,:,2),[]); title('Forward Transf. in y direction');

% Calculate strain tensors
E = strain(F(:,:,1),F(:,:,2));
% Show the strain tensors
figure,
subplot(2,2,1), imshow(E(:,:,1,1),[-1 1]); title('Strain Tensors Exx');
subplot(2,2,2), imshow(E(:,:,1,2),[-1 1]); title('Strain Tensors Exy');
subplot(2,2,3), imshow(E(:,:,2,1),[-1 1]); title('Strain Tensors Eyx');
subplot(2,2,4), imshow(E(:,:,2,2),[-1 1]); title('Strain Tensors Eyy');

end


function value = scnrm2 ( n, x, incx )

%*****************************************************************************80
%
%% SCNRM2 returns the euclidean norm of a complex vector.
%
%  Discussion:
%
%    SCNRM2 := sqrt ( sum ( conjg ( x(1:n) ) * x(1:n) ) )
%            = sqrt ( dot_product ( x(1:n), x(1:n) ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for FORTRAN usage,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, pages 308-323, 1979.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, complex X(*), the vector.
%
%    Input, integer INCX, the increment between successive entries of X.
%
%    Output, real VALUE, the norm of the vector.
%
  value = sqrt ( sum ( ( real ( x(1:incx:1+(n-1)*incx) ) ).^2 ) ...
               + sum ( ( imag ( x(1:incx:1+(n-1)*incx) ) ).^2 ) );

  return
end


function [ux,u,m] = applypca(X)
% [ux,u,m] = applypca(X)
%
% finds principal components of
%
% input:
%  x  input data (each column is an input vector)
%
% output:
%  ux  data projected onto pca (1st row=leading principal component)
%  u   the orthogonal matrix with all principal components
%  m   mean of the data
%
% You can recover the original data with
%
% xoriginal=u*ux+repmat(m,1,size(ux,2));
%
% See also pca
%
% copyright by Kilian Q. Weinberger, 2006

[u,v]=pca(X);
m=mean(X,2);
ux=u'*(X-repmat(m,1,size(X,2)));


function permindx = permute_setupperms(n,nperms)
% Set up permutations x observations matrix of observation indices for
% permutation test
%
% :Usage:
% ::
%
%    % approximate test with nperms obs.
%    permindx = permute_setupperms(n,nperms)
%    permindx = permute_setupperms(n,[])
%
%    % exact test (all permutations) of n observations
%    permindx = permute_setupperms(n)
%
% ..
%    Uses code from SnPM3b by Tom Nichols
%    adapted for this function by Tor Wager
% ..

  if nargin < 2 || isempty(nperms) || nperms == 0
    OUT.meth = 'exact';
    nperms = gamma(n+1);
    use_approximate = 0;
  else
    nperms = min(gamma(n+1),nperms);
    use_approximate = 1;

    if nperms > 50000
      warning('More than 50,000 permutations! May cause time/memory problems');
    end
  end

				% interactive
				% % nperms = gamma(n+1);
% % use_approximate = input(sprintf('%d Perms. Enter return for exact test, or num. of perms in random subset: ',nperms));
% % if isempty(use_approximate)
% %     use_approximate = 0;
% % else
% %     nperms = min(use_approximate,nperms);
% %     use_approximate = 1;
% % end

  if use_approximate
      % taken from snpm3b code, by Tom Nichols
      %-Approximate test :
      % Build up random subset of all (within n) permutations
      %===============================================================
    rand('seed',sum(100*clock))	%-Initialise random number generator
    permindx      = zeros(nperms,n);
    permindx(1,:) = 1+rem(0:(n-1), n);

    for i = 2:nperms+1
      permindx(i,:) = randperm(n);
    end
    b = unique(permindx, 'rows');
    if(size(b,1) ~= size(permindx, 1))
      while(size(b,1) ~= size(permindx, 1))
        permindx = b;
        for i = (size(permindx,1)+1):nperms+1
          permindx(i,:) = randperm(n);
        end
        b = unique(permindx, 'rows');
      end
      permindx = [b(1,:); b(randperm(size(b,1)-1)+1,:)];
    end

		       % null permutation is not included in perm indx
    permindx = permindx(2:end,:);
  else
      % taken from snpm3b code, by Tom Nichols
      %
      %-Full permutation test :
      % Build up exhaustive matrix of permutations
      %===============================================================
      %-Compute permutations for a single exchangability block
      %---------------------------------------------------------------
      %-Initialise Xblkpermindx & remaining numbers
    Xblkpermindx = [];
    lef = [1:n]';
%-Loop through numbers left to add to permutations, accumulating permindx
    for i = n:-1:1
				%-Expand Xblkpermindx & lef
      tmp = round(exp(gammaln(n+1)-gammaln(i+1)));
      Exp = meshgrid(1:tmp,1:i); Exp = Exp(:)';
      if ~isempty(Xblkpermindx), Xblkpermindx = Xblkpermindx(:,Exp); end
      lef = lef(:,Exp);
				%-Work out sampling for lef
      tmp1 = round(exp(gammaln(n+1)-gammaln(i+1)));
      tmp2 = round(exp(gammaln(n+1)-gammaln(i)));
      sam = 1+rem(0:i*tmp1-1,i) + ([1:tmp2]-1)*i;
			      %-Add samplings from lef to Xblkpermindx
      Xblkpermindx   = [Xblkpermindx; lef(sam)];
		      %-Delete sampled items from lef & condition size
      lef(sam) = [];
      tmp = round(exp(gammaln(n+1)-gammaln((i-1)+1)));
      lef = reshape(lef,(i-1),tmp);
		%NB:gamma(n+1)/gamma((i-1)+1) == size(Xblkpermindx,2);
    end
    clear lef Exp sam i
				%-Reorient so permutations are in rows
    permindx = Xblkpermindx';
  end

			     %-Check, condition and randomise permindx
%-----------------------------------------------------------------------
%-Check permindxs sum within Xblks to sum to 1
  if ~all(all(sum(permindx,2) == (n+1)*n/2 ))
    error('Invalid permindx computed!'), end

	%-Convert to full permutations from permutations within blocks
  nperms = size(permindx,1);

%-Randomise order of permindxs (except first) to allow interim analysis
  rand('seed',sum(100*clock))	%-Initialise random number generator
  permindx=[permindx(1,:);permindx(randperm(nperms-1)+1,:)];

			 %-Check first permutation is null permutation
  if ~use_approximate
    if ~all(permindx(1,:)==[1:n])
      error('permindx(1,:)~=[1:n]');
    end
  end

end


classdef hessenberg
% Functions to work with Hessenberg forms

methods(Static)

function [Q, R] = qr(A)
    % Givens method for computing the QR factorization A = QR of a Hessenberg matrix.
    % A is modified in the algorithm
    % GVL4: algorithm 5.2.5
    import spx.la.givens.rotation;
    [n,n] = size(A);
    % Space for saving the Q factor
    Q = eye(n, n);
    % iterate over each column from first to last but one.
    for j=1:n-1
        % We need to process only one pair of rows
        % The diagonal element and immediate sub-diagonal element.
        a = A(j,j);
        b = A(j+1,j);
        % Compute the needed rotation for making b 0.
        [c,s] = rotation(a,b);
        % Form the givens rotation matrix.
        G = [c s;-s c];
        % Rotate the two consecutive rows based submatrix
        A(j:j+1,j:n) = G'*A(j:j+1,j:n);
        % Rotate corresponding columns n Q matrix
        % by postmultiplication with G
        Q(:,j:j+1) = Q(:,j:j+1)*G;
    end
    % Return the triangular form
    R = A;
end % function

function [H, cs, ss] = qr_rq(H)
    % Takes a Hessenberg matrix H = QR and returns RQ
    % GVL4: algorithm 7.4.1
    import spx.la.givens.rotation;
    [n,n] = size(H);
    % Space for storing the c and s values
    cs = zeros(1, n);
    ss = zeros(1, n);
    % first the QR factorization
    % iterate over each column from first to last but one.
    for k=1:n-1
        % We need to process only one pair of rows
        % The diagonal element and immediate sub-diagonal element.
        a = H(k,k);
        b = H(k+1,k);
        % Compute the needed rotation for making b 0.
        [c,s] = rotation(a,b);
        % Store the rotation
        cs(k) = c;
        ss(k) = s;
        % Form the givens rotation matrix.
        G = [c s;-s c];
        % Rotate the two consecutive rows based submatrix
        H(k:k+1,k:n) = G'*H(k:k+1,k:n);
    end
    % Now the RQ formation
    for k=1:n-1
        G = [cs(k) ss(k); -ss(k) cs(k)];
        H(1:k+1,k:k+1) = H(1:k+1,k:k+1) * G;
    end
    % The product of these Givens rotations is also upper Hessenberg.
    % Product of two Hessenberg matrices is Hessenberg.
end % function

function [H, U0] = hess(A)
    % Reduction of a square matrix A into Hessenberg form
    % using Householder reflections
    % GVL4: algorithm 7.4.2
    [n,n] = size(A);
    import spx.la.house;
    % Iterate over columns of A
    for k=1:n-2
        % Compute the householder reflector for k-th column
        % covering only sub-diagonal elements
        [v,beta] = house.gen(A(k+1:n,k));
        % Update the submatrix of A by premultiplication with the Projector
        A(k+1:n,k:n) = A(k+1:n,k:n) - (beta*v)*(v'*A(k+1:n,k:n));
        % Postmultiply with the same projector without affecting
        % the 0's in the k-th column
        A(:,k+1:n) = A(:,k+1:n) - (A(:,k+1:n)*v)*(beta*v)';
        % Store the householder reflection vector
        A(k+2:n,k) = v(2:n-k);
    end
    % Extract the Hessenberg form
    H = triu(A,-1);
    if nargout > 1
        % Extract the orthogonal matrix U_0 such that
        % A = U_0 H U_0^T
        U0 = eye(n,n);
        U0(2:n,2:n) = house.q_back_accum_full(A(2:n,1:n-1));
    end
end % function

function [H, i1, i2] = backsearch(H, z)
    % Searches for a subdiagonal entry in H which is
    % negligible. From last row to first row.
    if nargin < 2
        z = size(H, 2);
    end
    i1 = z;
    i2 = z;
    % Compute the Frobenius norm of H
    h_norm = norm(H, 'fro');
    % The threshold for negligible entries
    thr = eps * h_norm
    % iterate over diagonal entries backwards
    while i1 >1
        % check for negligible subdiagonal entry.
        fprintf('%d, %d, %e\n', i1, i1-1, H(i1, i1-1));
        if (abs(H(i1, i1-1)) < thr )
            % Make this entry zero
            H(i1, i1-1) = 0;
            if (i1 == i2)
                i2 = i1 - 1;
                i1 = i1 - 1;
            else
                return;
            end
        else
            i1 = i1 - 1;
        end

    end
end

end % methods

end % classdef


function [xc,mode] = XCorr1(x,y,varargin)

%XCorr1 - Compute cross-correlograms and their modes for pairs of curves.
%
%  USAGE
%
%    [xc,mode] = XCorr1(x,y,<options>)
%
%    x              first set of curves (MxN matrix: M curves, N bins)
%    y              second set of curves
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'type'        'linear' for linear curves (default), 'circular' otherwise
%    =========================================================================
%

% Copyright (C) 2012 by Michaël Zugaro
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.

% Defaults
type = 'linear';

% Check number of parameters
if nargin < 2 | mod(length(varargin),2) ~= 0,
  error('Incorrect number of parameters (type ''help XCorr1'' for details).');
end

% Check parameter sizes
if ~isdmatrix(x) || ~isdmatrix(y),
	error('Both sets of curves should be MxN matrices (type ''help XCorr1'' for details).');
end
if ~all(size(x)==size(y)),
	error('Both sets of curves should have the same sizes (type ''help XCorr1'' for details).');
end

% Parse parameter list
for i = 1:2:length(varargin),
	if ~ischar(varargin{i}),
		error(['Parameter ' num2str(i+2) ' is not a property (type ''help XCorr1'' for details).']);
	end
	switch(lower(varargin{i})),
		case 'type',
			type = varargin{i+1};
			if ~isstring_FMAT(type,'linear','circular'),
				error('Incorrect value for property ''type'' (type ''help XCorr1'' for details).');
			end
		otherwise,
			error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help XCorr1'' for details).']);
	end
end

[m,n] = size(x);

if strcmp(type,'linear'),
	xc = nan(m,2*n-1);
	for i = 1:m,
		% Cross-correlagram i
		xci = xcorr(x(i,:),y(i,:),'coeff');
		% mode of cross-correlogram i (in [-1..1])
		mi = round(mean(find(xci==max(xci))));
		mi = mi/n-1;
		% Store in matrices
		xc(i,:) = xci;
		mode(i,1) = mi;
	end
else
	xc = nan(m,2*n-1);
	for i = 1:m,
		% Cross-correlagram i
		xci = xcorr(x(i,:),y(i,:),'coeff');
		% mode of cross-correlogram i (in [-1..1])
		mi = round(mean(find(xci==max(xci))));
		mi = mi/n-1;
		% For circular curves, very large modes must be corrected
		if abs(mi) > 0.5,
			if mi > 0,
				xci = [xci(n+1:end) xci(1:n)];
				mi = mi - 1;
			else
				xci = [xci(n:end) xci(1:n-1)];
				mi = mi +1;
			end
		end
		xc(i,:) = xci;
		mode(i,1) = mi;
	end
end

%  	xc = nan(m,2*n-1);
%  	for i = 1:m,
%  		% Cross-correlagram i
%  		X = repmat(x(i,:),n,1);
%  		Y = repmat(y(i,:),1,n+1);
%  		Y(n+1:n+1:end) = [];
%  		Y = reshape(Y,n,[]);
%  		xci = sum(X.*Y,2)';
%  		xci = [xci(2:end) xci];
%  		% mode of cross-correlogram i (in [0..1])
%  		% (we need to compute mean values of x, but x is circular, so we transform it to angles)
%  		a = find(xci==max(xci));
%  		a = exp(j*a*2*pi/n);
%  		mi = round(wrap(angle(mean(a)),2)/(2*pi)*n)/n-1;
%  		if abs(mi) > 0.5,
%  			if mi > 0,
%  				mi = mi - 1;
%  			else
%  				mi = mi +1;
%  			end
%  		end
%  		% Zero outside primary field
%  		m = round((mi+1)*n);
%  		h = ceil(n/2);
%  		xci(m+h:end) = 0;
%  		xci(1:m-h) = 0;
%  		% Store in matrices
%  		xc(i,:) = xci;
%  		mode(i,1) = mi;
%  	end
%  	% Scale so that autocorrelation at shift 0 is 1
%  	x0 = sum(x.^2,2);
%  	y0 = sum(y.^2,2);
%  	scale = sqrt(x0.*y0);
%  	xc = xc./repmat(scale,1,2*n-1);
%
%  	end


function pass = test_nonlinSysDampingBreaks_C2(pref)
% Test 2x2 system (sin/cos) where damping is required
%
% Asgeir Birkisson, May 2014.

if ( nargin == 0 )
    pref = cheboppref;
end

tol = 1e-10;

%% Piecewise:
d = [-pi 0 pi];
x = chebfun('x',d);
f = [ 0*x ; 0*x ];

%% CHEBCOLLOC2
pref.discretization = @chebcolloc2;

A = chebop(@(x,u,v) [u - diff(v,2); diff(u,2) + cos(v)], d);
A.lbc = @(u,v) [u-1/2; v+1/4];
A.rbc = @(u,v) [v-1/4; u+1/2];

[u1, u2, info] = solvebvp(A, f, pref);

% Want to check BCs as well.
bcFunLeft = chebfun(A.lbc(u1,u2));
bcFunRight = chebfun(A.rbc(u1,u2));

% And check that we're continuous over breakpoint
u1jump = jump(u1, 0);
u2jump = jump(u2, 0);

pass(1) = abs(info.error) < tol;
pass(2) = norm(bcFunLeft(d(1))) < tol && norm(bcFunRight(d(end))) < tol;
pass(3) = norm(u1jump) < tol && norm(u2jump) < tol;

end


function y=rdct(x,n,a,b)
%RDCT     Discrete cosine transform of real data Y=(X,N,A,B)
% Data is truncated/padded to length N.
%
% This routine is equivalent to multiplying by the matrix
%
%   rdct(eye(n)) = diag([sqrt(2)*B/A repmat(2/A,1,n-1)]) * cos((0:n-1)'*(0.5:n)*pi/n)
%
% Default values of the scaling factors are A=sqrt(2N) and B=1 which
% results in an orthogonal matrix. Other common values are A=1 or N and/or B=1 or sqrt(2).
% If b~=1 then the columns are no longer orthogonal.
%
% see IRDCT for the inverse transform

% BUG: in line 51 we should do chopping after transform and not before

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

fl=size(x,1)==1;
if fl x=x(:); end
[m,k]=size(x);
if nargin<2 n=m;
end
if nargin<4 b=1;
    if nargin<3 a=sqrt(2*n);
    end
    end
if n>m x=[x; zeros(n-m,k)];
elseif n<m x(n+1:m,:)=[];
end

x=[x(1:2:n,:); x(2*fix(n/2):-2:2,:)];
z=[sqrt(2) 2*exp((-0.5i*pi/n)*(1:n-1))].';
y=real(fft(x).*z(:,ones(1,k)))/a;
y(1,:)=y(1,:)*b;
if fl y=y.'; end


function [prices, stdErrs] = Price_MC_Hindsight_Strikes_func(Spath, call, strikes, M, mult, disc)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Calculates Hindsight option prices for vector of strikes, given the simulatd paths
% Author: Justin Lars Kirkby
%
% -----------------
% Params
% -----------------
% Contract Types
% Fixed Strike (Hindsight) Put: (W - min{S_m: 0<=m<=M})^+
% Fixed Strike (Hindsight) Call: (max{S_m: 0<=m<=M} - W)^+
%
% Spath = paths of underlying, dimension N_sim x M+1, where M = number of time steps (since includes S_0)
% call = 1 for call (else put)
% Kvec = strike vector
% M = number of monitoring points, e.g. 252 for "daily" monitoring
% mult = time partitioning multiplier to reduce bias (e.g. mult = 2 or 5)
% S_0 = initial price
% disc = discount factor (e.g. exp(-r*T))
% T = Time (in years)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

N_sim = size(Spath,1);  % number of paths

M_mult = M*mult;  %time partitioning to reduce bias

prices = zeros(length(strikes),1);
stdErrs = zeros(length(strikes),1); %TODO: add code to return std errros

if call == 1
    curr_max = zeros(N_sim, 1);
    for n = 1:N_sim
        curr_max(n) = max(Spath(n, 1:mult:M_mult+1));
    end
else  % find_min
    curr_min = zeros(N_sim, 1);
    for n = 1:N_sim
        curr_min(n) = min(Spath(n, 1:mult:M_mult+1));
    end
end

for k = 1:length(strikes)
    K = strikes(k);
    if call ==1  % Fixed Strike (Hindsight) Call: (max{S_m: 0<=m<=M} - W)^+
       payoffs  = max(0, curr_max - K);

    else  % Fixed Strike (Hindsight) Put: (W - min{S_m: 0<=m<=M})^+
        payoffs  = max(0, K - curr_min);
    end
    prices(k) = disc*mean(payoffs);
    stdErrs(k) = disc*std(payoffs) / sqrt(N_sim);

end

end


function rate_na_lt = awgn_na_lt(snr_db,nn,error)

P=10^(snr_db/10);

rate_na_lt=zeros(1,length(nn));

%find the minimum blocklength required for the long-term power constraint to be beneficial
C=log(1+P);
V = 1-1/(1+P)^2;
n_min = ( (1+P)/P*sqrt(2*3.1415926*V)*(1-error)*exp(qfuncinv(error)^2/2) + qfuncinv(error)/(1+P)^2/sqrt(V))^2;

n_index=find(nn <= n_min);

% for every n<=n_min, the normal approx. for the st power constraint coincides
if length(n_index)>0
    rat_na_lt(n_index) = (log(1+P) - sqrt(1-(1+P)^(-2))./sqrt(nn(n_index)) + 0.5*log(nn(n_index))./nn(n_index))/log(2);
end

R_st = C - sqrt(V./nn)*qfuncinv(error);
C_lt=log(1+P/(1-error));

index_large_n = find(nn>n_min);

for ii=index_large_n
    n=nn(ii);
    for R_star = R_st(ii):0.0001:C_lt
        omega = P: 0.00001:P*1.2; %note that larger endpoing for omega (P*1.2) gives a better estimate for rate_na;

        q_arg=sqrt(n)*(log(1+omega)-R_star)./sqrt(1-(1+omega).^(-2)) ;

        error_temp = min( 1-P./omega + P.*qfunc( q_arg)./omega);
        if error_temp> error
            rate_na_lt(ii)= (R_star +0.5*log(n)./n)./log(2) ;
            break
        end
    end
end


function [f] = myrosenbrock(x)
f = 100*(x(2)-x(1)^2)^2+(1-x(1))^2;


function p = psnr( I, J )

% Compute PSNR between images
% Peak signal-to-noise ratio; built-in function not used since it
% assumes pixel values in interval [0, 255].
%
% Syntax:
%	p = psnr( I, J )
%
%
% Input:
%	I : Original image.
%
%	J : Denoised image.
%
%
% Output:
%	p : PSNR between I and J.

if ~isequal( size(I), size(J) )
	error('Images must be of the same size');
end

pixel_range = max(I(:)) - min(I(:));
MSE = norm( I - J, 'fro' );

p = 20*log10( pixel_range * sqrt(numel(I)) / MSE );

end


function SD
clc;
clear;
close all;
%% the example fuction
datasize=200;
var_noise=0.1;
x=linspace(-10,10,datasize)';
    f=sin(x)./x;
noise=var_noise*randn(datasize,1);
y=f+noise;
x_raw=x;
plot(x_raw,f,'r-','LineWidth',2),hold on;
%% the training data samples
Inset=30;
dataindex=randsample(datasize,Inset);
xI=x(dataindex);
yI=y(dataindex);
plot(xI,yI,'b+'),hold on;
%  x(dataindex)=[];
%  y(dataindex)=[];
x=xI;
y=yI;
K_mat=Kernel_func(x,x,0.7,1.2)+var_noise*eye(Inset);
% [vecmat,valmat]=eig(K_mat);
% valmat=diag(valmat);
% [valmat,ix]=sort(valmat,'descend');
% vecmat=vecmat(:,ix);
L=chol(K_mat,'lower');
%%
testnum=500;
x_test=linspace(-15,15,testnum)';
K_test_train=Kernel_func(x_test,x,0.7,1.2);
K_test=Kernel_func(x_test,x_test,0.7,1.2);

f_mean=K_test_train*(L'\(L\y));
for i=1:testnum
K_ss=Kernel_func(x_test(i),x_test(i),0.7,1.2);
K_s=Kernel_func(x_test(i),x,0.7,1.2);
v=L\K_s';
  f_var(i)=K_ss-v'*v;
end
f_var=f_var';
plot(x_test,f_mean,'b-','LineWidth',2),hold on;
%%
x_area=[x_test;flipdim(x_test,1)];
ybound_std=[(f_mean+1.96*sqrt(f_var));flipdim(f_mean-1.96*sqrt(f_var),1)];
fill(x_area,ybound_std,[187 255 0]/255,'EdgeColor','none');
plot(x_raw,f,'r-','LineWidth',2),hold on;
plot(xI,yI,'b+'),hold on;
plot(x_test,f_mean,'b-','LineWidth',2),hold on;
%%
legend1=legend('target function','training points','preditive distribution','95% confidence interval');
 set(legend1,'Box','off','Color','none',...
     'Location','NorthEast');
xlabel('SD with 30 points ')
set(gca,'XTick',[-15:3:15])
matlab2tikz( 'SubData30.tex' )


function problem = gen_category_registration(problem)
%% Generate random category registration problem
%% Heng Yang, July 04, 2021

if ~isfield(problem,'N'); error('Please use problem.N to specify number of keypoints.'); end
if ~isfield(problem,'K'); problem.K = 2; end
if ~isfield(problem,'noiseSigma'); problem.noiseSigma = 0.01; end
if ~isfield(problem,'outlierRatio'); problem.outlierRatio = 0.0; end
if ~isfield(problem,'intraRadius'); problem.intraRadius = 0.2; end
if ~isfield(problem,'translationBound'); problem.translationBound = 10.0; end

N               = problem.N;
K               = problem.K;
noiseSigma      = problem.noiseSigma;
outlierRatio    = problem.outlierRatio;
intraRadius     = problem.intraRadius;
translationBound= problem.translationBound;

% generate a mean shape centered at zero
mean_shape = randn(3,N);
mean_shape = mean_shape - mean(mean_shape,2);

shapes     = zeros(3,N,K);
for k = 1:K
    shapes(:,:,k)  = mean_shape + intraRadius * randn(3,N);
end

% ground truth c, R, t
c_gt           = rand(K,1);
c_gt           = c_gt/sum(c_gt); % c sum up to one
R_gt           = rand_rotation;
t_gt           = randn(3,1); t_gt = t_gt/norm(t_gt);
t_gt           = translationBound * rand * t_gt;

% generate a noisy scene
shape          = combine_shapes(shapes,c_gt);
scene          = R_gt * shape + t_gt + noiseSigma * randn(3,N);

% generate outliers
nrOutliers     = round(N*outlierRatio);
if nrOutliers > 0
    fprintf('Category registration: generate %d outliers.\n',nrOutliers);
    outlierIDs  = N-nrOutliers+1:N;
    outliers    = randn(3,nrOutliers);
    scene(:,outlierIDs) = outliers;
else
    outlierIDs   = [];
end

theta_gt   = ones(N,1);
theta_gt(outlierIDs) = -1;

problem.type                = 'category registration';
problem.shape               = shape;
problem.shapes              = shapes;
problem.scene               = scene;
problem.nrOutliers          = nrOutliers;
problem.outlierIDs          = outlierIDs;
problem.c_gt                = c_gt;
problem.R_gt                = R_gt;
problem.t_gt                = t_gt;
problem.cBound              = 1.0;
problem.theta_gt            = theta_gt;

noiseBoundSq                = max(4e-2, noiseSigma^2 * chi2inv(0.99,3));
problem.noiseBoundSq        = noiseBoundSq;
problem.noiseBound          = sqrt(problem.noiseBoundSq);

fprintf('N: %d, num outliers: %d, noise bound: %g, translation bound: %g, c bound: %g\n',...
    problem.N,problem.nrOutliers,problem.noiseBound,problem.translationBound,problem.cBound);
end


function value = p16_exact ( dim_num )

%*****************************************************************************80
%
%% P16_EXACT returns the exact integral for problem 16.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Output, real VALUE, the exact value of the integral.
%

%
%  Get the limits of integration.
%
  [ a, b ] = p16_lim ( dim_num );
%
%  Get the location of Z.
%
  z = [];
  z = p16_r8vec ( 'G', 'Z', dim_num, z );
%
%  The value of the DIM_NUM dimensional integral can be broken down
%  into the weighted sum of 1 dimensional integrals.
%
  value = 0.0;

  volume = prod ( b(1:dim_num) - a(1:dim_num) );

  for i = 1 : dim_num
%
%  Z < A < B
%
    if ( z(i) < a(i) )

      integral = 0.5 * ( b(i) - a(i) ) * ( b(i) + a(i) - 2.0 * z(i) );
%
%  A < Z < B
%
    elseif ( z(i) < b(i) )

      integral = 0.5 * ( ( b(i) - z(i) )^2 + ( z(i) - a(i) )^2 );
%
%  A < B < Z
%
    else

      integral = 0.5 * ( b(i) - a(i) ) * ( 2.0 * z(i) - a(i) - b(i) );

    end

    value = value + volume * integral / ( b(i) - a(i) );

  end

  return
end


function f = goldstein_price_xy ( x, y )

%*****************************************************************************80
%
%% GOLDSTEIN_PRICE_XY evaluates the Goldstein-Price polynomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Zbigniew Michalewicz,
%    Genetic Algorithms + Data Structures = Evolution Programs,
%    Third Edition,
%    Springer Verlag, 1996,
%    ISBN: 3-540-60676-9,
%    LC: QA76.618.M53.
%
%  Parameters:
%
%    Input, real X, Y, the arguments of the function.
%
%    Output, real F, the value of the function at X.
%
  a = x + y + 1.0;

  b = 19.0 - 14.0 * x + 3.0 * x * x - 14.0 * y ...
    + 6.0 * x * y + 3.0 * y * y;

  c = 2.0 * x - 3.0 * y;

  d = 18.0 - 32.0 * x + 12.0 * x * x + 48.0 * y ...
    - 36.0 * x * y + 27.0 * y * y;

  f = ( 1.0 + a * a * b ) * ( 30.0 + c * c * d );

  return
end


function v = fdm_2d_vector(n0,f_str)
%
%  Generates a vector v which contains the values of a function f(x,y)
%  on an equidistant grid in the interior of the unit square. The grid
%  points are numbered consistently with those used in the function
%  'fdm_2d_matrix'.
%
%  This function is just used as an easy way to generate test problems
%  rather than to solve PDEs.
%
%  Calling sequence:
%
%    v = fdm_2d_vector( n0, f_str)
%
%  Input:
%
%    n0        number of inner grid points in each dimension;
%    f_str     string describing the function f in the space variables 'x'
%              and 'y', e.g., f_str = 'sin(x+2*y)+3'.
%
%  Output:
%
%    v         vector of order n = n0^2 containing the values of f(x,y).
%
%
%  LYAPACK 1.0 (Thilo Penzl, May 1999)

% Input data not completely checked!

na = nargin;

if na~=2
  error('Wrong number of input parameters.');
end

h = 1.0/(n0+1);

n2 = n0*n0;

v = zeros(n2,1);

i = 0;

for iy = 1:n0
  y = iy*h;
  for ix = 1:n0
    x = ix*h;
    i = i+1;
    v(i) = eval(f_str);
  end
end


function [Xs,Ys] = ml_get_grid(X,dims,num_pts )
%ML_GET_GRID  Returns Xs and Ys meshgrid with limits obtained from X.
%
%   input -----------------------------------------------------------------
%
%       o X : (N x D), data
%
%       o dims : (1 x 2), dimensions to create grid from
%

D = size(X,2);

if D == 1
    max_d1  = max(X);
    min_d1  = min(X);
    Xs      = linspace(min_d1,max_d1,num_pts);
    Ys      = [];
else
    max_d1  = max(X(:,dims(1)));
    min_d1  = min(X(:,dims(1)));
    max_d2  = max(X(:,dims(2)));
    min_d2  = min(X(:,dims(2)));
    [Xs,Ys] = meshgrid(linspace(min_d1,max_d1,num_pts),linspace(min_d2,max_d2,num_pts));
end

end


function [dPriors,dMu,dSigma,res ] = log_gmm_div(X,Priors,Mu,Sigma )
%LOG_GMM_DIV Derivative of the Log of a Guassian Mixture Model
%
%
%   input -----------------------------------------------------------------
%
%       o X         : (D x N),      Samples
%
%       o Priors    : (1 x K),      Weights
%
%       o Mu        : (D x K),      Means
%
%       o Sigma     : (D x D x K),  Covariance
%
%   output ----------------------------------------------------------------
%
%
%
%

[D,N]   = size(X);

K       = size(Priors,2);

% (N x 1)
L       = gmm_pdf(X,Priors,Mu,Sigma);

dPriors = zeros(1,K);
dMu     = zeros(D,K);
dSigma  = zeros(D,D,K);

% for each parameter k, compute the gradient for each point and sum

k_prior = 1./K;

res = zeros(K,N);
% compute gradient for Priors = responsability factor
for k=1:K
    res(k,:) =  Priors(k) .* gaussPDF(X,Mu(:,k),Sigma(:,:,k)) ./ L;
end

for k=1:K
    %invSigma_k    = inv(Sigma(:,:,k));

   % dPriors(k)    =  (1/N) .* sum(gaussPDF(X,Mu(:,k),Sigma(:,:,k)) ./ L);
    Nk            =  sum(res(k,:));
    dPriors(k)    =  Nk/N;
    dMu(:,k)      =  (1/Nk) .* sum( repmat(res(k,:)',1,D) .* X' ,1)';
    diff          =  X - repmat(Mu(:,K),1,N); % (D x N)
    %                                   (N x N) * (N x D)

    for n=1:N
         dSigma(:,:,k) = dSigma(:,:,k) +  res(k,n) * (X(:,n) - Mu(:,K)) * (X(:,n) - Mu(:,K))';
    end
     dSigma(:,:,k)  =  sqrt((1/Nk) .* dSigma(:,:,k));
   %  (1/Nk) .*  diff * diag(res(k,:)) * diff';

   % dSigma(:,:,k) =  dPriors(k) * 0.5 *(-invSigma_k + invSigma_k * Cov * invSigma_k );
    %dSigma(:,:,k) = 0.5 * ( dSigma(:,:,k) +  dSigma(:,:,k)');
end

%dPriors = dPriors ./ sum(dPriors);

end


function an=euler2axan(phi,theta,psi,M)
% From Euler angles (phi,theta,psi) to axis angle (x y z alpha)/(gamma,delta):

% v=[(cos(al-gm)+1)*(1-cos(bt));
%    sin(al-gm)*(1-cos(bt));
%    sin(bt)*(sin(al)+sin(gm))]

cost=cos(theta);
sint=cos(theta);
omcost=(1-cost);
if sint==0
    x=0;
    y=0;
    z=1;
else
    x = (cos(phi-psi)+1)*omcost;
    y = sin(phi-psi)*omcost;
    z = sint*(sin(phi)+sin(psi));
end

if (x==0)&&(y==0)&&(z==0)
    x=1;
    y=0;
    z=0;
else
    vn=sqrt(x^2+y^2+z^2);
    x=x/vn;
    y=y/vn;
    z=z/vn;
end

v=[x y z]';

%[THETA,PHI,R] = cart2sph(X,Y,Z)

[gamma,delta,tmp] = cart2sph(x,y,z);

cppp=cos(phi+psi);
Tr=cppp+cppp*cost+cost;
alpha=atan2(-(M(1,2)*v(3)+M(2,3)*v(1)+M(3,1)*v(2)-M(1,3)*v(2)-M(3,2)*v(1)-M(2,1)*v(3)),Tr-1);
% if (-(M(1,2)*v(3)+M(2,3)*v(1)+M(3,1)*v(2)-M(1,3)*v(2)-M(3,2)*v(1)-M(2,1)*v(3)))>=0
%     alpha=acos((Tr-1)/2);
% else
%     alpha=-acos((Tr-1)/2);
% end

an={{gamma,delta,alpha},v};


function [Y,T]=derivs(x,dim)
%Syntax: [Y,T]=derivs(x,dim)
%____________________________
%
% The phase space reconstruction of a time series x whith the derivatives approach,
% in embedding dimension m.
%
% Y is the trajectory matrix in the reconstructed phase space.
% T is the phase space length.
% x is the time series.
% dim is the embedding dimension.
%
%
% Reference:
%
% Packard N H, Cruchfield J P, Farmer J D, Shaw R S (1980): Geometry from a Time
% Series. Physical Review Letters 45: 712-715
%
%
% Alexandros Leontitsis
% Department of Education
% University of Ioannina
% 45110 - Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% 11 Mar 2001.

if nargin<1 | isempty(x)==1
   error('You should provide a time series.');
else
   % x must be a vector
   if min(size(x))>1
      error('Invalid time series.');
   end
   x=x(:);
   % N is the time series length
   N=length(x);
end

if nargin<2 | isempty(dim)==1
    dim=2;
else
    % dim must be scalar
    if sum(size(dim))>2
        error('dim must be scalar.');
    end
    % dim must be an integer
    if dim-round(dim)~=0
        error('dim must be an integer.');
    end
    % dim must be positive
    if dim<=0
        error('dim must be positive.');
    end
end

% Initialize the phase space
Y=zeros(N,dim);

% Phase space reconstruction with the derivatives approach
Y(:,1)=x;
for i=2:dim
    Y(:,i)=[diff(Y(:,i-1));0];
end

% Total points on phase space
T=N-(dim-1);

% Remove the meaningless points
Y(T+1:end,:)=[];


function bvec2 = bvec_complement2 ( n, bvec1 )

%*****************************************************************************80
%
%% BVEC_COMPLEMENT2 computes the two's complement of a binary vector.
%
%  Discussion:
%
%    A BVEC is an integer vector of binary digits, intended to
%    represent an integer.  BVEC(1) is the units digit, BVEC(N-1)
%    is the coefficient of 2^(N-2), and BVEC(N) contains sign
%    information.  It is 0 if the number is positive, and 1 if
%    the number is negative.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 November 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the length of the vectors.
%
%    Input, integer BVEC1(N), the vector to be complemented.
%
%    Output, integer BVEC2(N), the two's complemented vector.
%
  base = 2;

  bvec3(1:n) = ( base - 1 ) - bvec1(1:n);

  bvec4(1) = 1;
  bvec4(2:n) = 0;

  bvec2 = bvec_add ( n, bvec3, bvec4 );

  return
end


function fd1d_heat_implicit_test03 ( )

%*****************************************************************************80
%
%% FD1D_HEAT_IMPLICIT_TEST03 does a simple test problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FD1D_HEAT_IMPLICIT_TEST03:\n' );
  fprintf ( 1, '  Compute an approximate solution to the time-dependent\n' );
  fprintf ( 1, '  one dimensional heat equation:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    dH/dt - K * d2H/dx2 = f(x,t)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Run a simple test case.\n' );
%
%  Heat coefficient.
%
  k = k_test03 ( );
%
%  X_NUM is the number of equally spaced nodes to use between 0 and 1.
%
  x_num = 21;
  x_min = -5.0;
  x_max = +5.0;
  dx = ( x_max - x_min ) / ( x_num - 1 );
  x = linspace ( x_min, x_max, x_num );
%
%  T_NUM is the number of equally spaced time points between 0 and 10.0.
%
  t_num = 81;
  t_min = 0.0;
  t_max = 4.0;
  dt = ( t_max - t_min ) / ( t_num - 1 );
  t = linspace ( t_min, t_max, t_num );
%
%  Get the CFL coefficient.
%
  cfl = fd1d_heat_implicit_cfl ( k, t_num, t_min, t_max, x_num, x_min, x_max );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of X nodes = %d\n', x_num );
  fprintf ( 1, '  X interval is [%f,%f]\n', x_min, x_max );
  fprintf ( 1, '  X spacing is %f\n', dx );
  fprintf ( 1, '  Number of T values = %d\n', t_num );
  fprintf ( 1, '  T interval is [%f,%f]\n', t_min, t_max );
  fprintf ( 1, '  T spacing is %f\n', dt );
  fprintf ( 1, '  Constant K = %g\n', k );
  fprintf ( 1, '  CFL coefficient = %g\n', cfl );
%
%  Get the system matrix.
%
  a = fd1d_heat_implicit_matrix ( x_num, cfl );

  hmat = zeros ( x_num, t_num );

  for j = 1 : t_num
    if ( j == 1 )
      h = ic_test03 ( x_num, x, t(j) );
      h = bc_test03 ( x_num, x, t(j), h );
    else
      h = fd1d_heat_implicit ( a, x_num, x, t(j-1), dt, cfl, @rhs_test03, @bc_test03, h );
    end
    hmat(1:x_num,j) = h(1:x_num);
  end
%
%  Plot the data.
%
  figure ( 3 )
  [ tmat, xmat ] = meshgrid ( t, x );
  mesh ( tmat, xmat, hmat );
  title ( 'H(X,T) for TEST03 computed by FD1D\_HEAT\_IMPLICIT' );
  xlabel ( '<-- Time -->' );
  ylabel ( '<-- X -->' );
  zlabel ( '<--  H(X,T) -->' );
%
%  Write the data to files.
%
  r8mat_write ( 'h_test03.txt', x_num, t_num, hmat );
  r8vec_write ( 't_test03.txt', t_num, t );
  r8vec_write ( 'x_test03.txt', x_num, x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  H(X,T) written to "h_test03.txt"\n' );
  fprintf ( 1, '  T values written to "t_test03.txt"\n' );
  fprintf ( 1, '  X values written to "x_test3.txt"\n' );
  return
end


function jacobi_eigenvalue_test02 ( )

%*****************************************************************************80
%
%% JACOBI_EIGENVALUE_TEST02 uses a 4x4 test matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 July 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'JACOBI_EIGENVALUE_TEST02\n' );
  fprintf ( 1, '  For a symmetric matrix A,\n' );
  fprintf ( 1, '  JACOBI_EIGENVALUE computes the eigenvalues D\n' );
  fprintf ( 1, '  and eigenvectors V so that A * V = D * V.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  As a "sanity check", input a diagonal matrix.\n' );

  n = 4;

  a = [ ...
      4.0, 0.0, 0.0, 0.0;
      0.0, 1.0, 0.0, 0.0;
      0.0, 0.0, 3.0, 0.0;
      0.0, 0.0, 0.0, 2.0 ];

  r8mat_print ( n, n, a, '  Input matrix A:' );

  it_max = 100;

  [ v, d, it_num, rot_num ] = jacobi_eigenvalue ( n, a, it_max );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations = %d\n', it_num );
  fprintf ( 1, '  Number of rotations  = %d\n', rot_num );

  r8vec_print ( n, d, '  Eigenvalues D:' );

  r8mat_print ( n, n, v, '  Eigenvector matrix V:' );
%
%  Compute eigentest.
%
  error_frobenius = r8mat_is_eigen_right ( n, n, a, v, d );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Frobenius norm error in eigensystem A*V-D*V = %g\n', ...
    error_frobenius );

  return
end


function op = prox_max( q )

%PROX_MAX    Entry-wise maximum element.
%    OP = PROX_MAX( q ) implements the nonsmooth function
%        OP(X) = q * max( X(:) ).
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    then it must be a positive real scalar.
% Dual: proj_simplex.m (at least if X is a vector)
% See also proj_simplex, prox_linf, prox_linf

if nargin == 0,
	q = 1;
elseif ~isnumeric( q ) || ~isreal( q ) || numel( q ) ~= 1 || q <= 0,
	error( 'Argument must be positive.' );
end
op = @(varargin)prox_lmax_q( q, varargin{:} );

function [ v, x ] = prox_lmax_q( q, x, t )
if nargin < 2,
    error( 'Not enough arguments.' );
end
% We have two options when input x is a matrix:
%   Does the user want to treat it as x(:),
%   Or does the user want to treat each column separately?
% Most other functions (e.g. l1, linf) treat it as x(:)
% so that will be the default. However, we leave it
% as a hard-coded option so that the user can change it
% if they want.
% (Note: the dual function, proj_simplex, vectorizes it)
VECTORIZE = true;
% right now, we do not have the non-vectorized version implemented.

tau = max( x(:) );
if nargin == 3,
    s   = sort( nonzeros(x), 'descend' );
%     s   = sort( x, 'descend' ); % 'nonzeros' does a x(:) operation
    cs  = cumsum(s);
    ndx = find( cs - (1:numel(s))' .* [s(2:end);0] >= t * q, 1 );
    if ~isempty( ndx ),
        tau = ( cs(ndx) - t * q ) / ndx;
        x   = x .* ( tau ./ max( x, tau ) );
    else
        x(:) = 0;  % adding Oct 21
        tau = 0;
    end
elseif nargout == 2,
    error( 'This function is not differentiable.' );
end
v = q * tau;

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function f_lim = sphArrayNoiseThreshold(R, Nmic, maxG_db, maxN, arrayType, dirCoeff)
%SPHARRAYNOISETHRESHOLD Returns freq.limits for noise amplification in an SMA
%
%   SPHARRAYNOISETHRESHOLD returns the frequencies that the noise in the
%   output channels of a SMA, after performing the SHT and equalization of
%   the output signals, reaches a certain user-defined threshold maxG_db.
%   The frequencies are computed only at the lower range of each order,
%   where its response decays rapidly, ignoring for example the nulls of an
%   open array at the higher frequencies. The estimation of the limits are
%   based on a linear approximation of the log-log response found e.g. in
%
%       Sector-based Parametric Sound Field Reproduction in the Spherical Harmonic Domain
%       A Politis, J Vilkamo, V Pulkki
%       IEEE Journal of Selected Topics in Signal Processing 9 (5), 852 - 866
%
%   Inputs:
%         R:          array radius
%         Nmic:       number of microphones
%         maxG_db:    max allowed amplification for the noise level,
%               maxG_db = 20*log10(maxG)
%         maxN:       maximum order that the array supports
%         arrayType:  'open', 'rigid' or 'directional' for an open or rigid spherical
%               array of omnidirectional microphones, or for an array of
%               first-order directional microphones
%         dirCoeff: (optional) if arrayType=='directional', then dirCoeff
%               expresses the first-order directivity of microphones
%               from 0-1 (1:omni, 0.5:cardioid, 0:dipole)
%
%         f_lim:    frequency points that the threhsold is reached,
%               for orders 1:maxN
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPHARRAYNOISETHRESHOLD.M - 11/7/2013
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<6
    dirCoeff = [];
end

c = 343;
f_lim = zeros(1,maxN);
for n=1:maxN
    bn = sphModalCoeffs(n, 1, arrayType, dirCoeff)/(4*pi);
    bn = bn(end);
    maxG = 10^(maxG_db/10);
    kR_lim = (maxG*Nmic*abs(bn).^2)^(-10*log10(2)/(6*n));
    f_lim(n) = kR_lim*c/(2*pi*R);
end

end


function q = mat2quat(mat,varargin)
% converts direction cosine matrix to quaternion
%
% Syntax
%   q = mat2quat(mat)
%
% Input
%  mat - vector of matrixes
%
% Output
%  q - @quaternion
%
% See also
%
% quaternion_matrix Euler axis2quat hr2quat
%
% Description
% Wertz says to the algo similar to this with largest divisor
%       q4 = 1/2*sqrt((1+mat(1,1)+mat(2,2)+mat(3,3)));
% Eqn 12-14a - c
%  Quat(1) = (mat(2,3)-mat(3,2))/q4/4;
%  Quat(2) = (mat(3,1)-mat(1,3))/q4/4;
%  Quat(3) = (mat(1,2)-mat(2,1))/q4/4;
%  Quat(4) = q4

Quat=zeros(4,1);

%Compute absolute values of the four quaternions from diags of Eqn 12-13
%absQ=0.5*sqrt([1 -1 -1;-1 1 -1;-1 -1 1;1 1 1]*diag(mat)+1);

absQ(1,:) = 0.5 * sqrt(1+mat(1,1,:)+mat(2,2,:)+mat(3,3,:));
absQ(2,:) = 0.5 * sqrt(1-mat(1,1,:)-mat(2,2,:)+mat(3,3,:));
absQ(3,:) = 0.5 * sqrt(1+mat(1,1,:)-mat(2,2,:)-mat(3,3,:));
absQ(4,:) = 0.5 * sqrt(1-mat(1,1,:)+mat(2,2,:)-mat(3,3,:));

[~,ind]=max(absQ); % Select biggest for best accuracy

qind = ind == 1;
if any(qind)
  Quat(1,qind)=absQ(1,qind);
  Quat(2,qind)=squeeze((mat(2,3,qind)-mat(3,2,qind))).'.*0.25./absQ(1,qind);
  Quat(3,qind)=squeeze((mat(3,1,qind)-mat(1,3,qind))).'.*0.25./absQ(1,qind);
  Quat(4,qind)=squeeze((mat(1,2,qind)-mat(2,1,qind))).'.*0.25./absQ(1,qind);
end

qind = ind == 2;
if any(qind)
  Quat(1,qind)=squeeze(mat(1,2,qind)-mat(2,1,qind)).'.*0.25./absQ(2,qind);
  Quat(2,qind)=squeeze(mat(3,1,qind)+mat(1,3,qind)).'.*0.25./absQ(2,qind);
  Quat(3,qind)=squeeze(mat(3,2,qind)+mat(2,3,qind)).'.*0.25./absQ(2,qind);
  Quat(4,qind)=absQ(2,qind);
end

qind = ind == 3;
if any(qind)
  Quat(1,qind)=squeeze(mat(2,3,qind)-mat(3,2,qind)).'.*0.25./absQ(3,qind);
  Quat(2,qind)=absQ(3,qind);
  Quat(3,qind)=squeeze(mat(1,2,qind)+mat(2,1,qind)).'.*0.25./absQ(3,qind);
  Quat(4,qind)=squeeze(mat(3,1,qind)+mat(1,3,qind)).'.*0.25./absQ(3,qind);
end

qind = ind == 4;
if any(qind)
  Quat(1,qind)=squeeze(mat(3,1,qind)-mat(1,3,qind)).'.*0.25./absQ(4,qind);
  Quat(2,qind)=squeeze(mat(1,2,qind)+mat(2,1,qind)).'.*0.25./absQ(4,qind);
  Quat(3,qind)=absQ(4,qind);
  Quat(4,qind)=squeeze(mat(2,3,qind)+mat(3,2,qind)).'.*0.25./absQ(4,qind);
end

q = quaternion(real(Quat));
q = q./norm(q);
q = inv(q).';


%% DEMO_volumetric_SED_eval
% This demo was developed as part of the paper:
% Moerman et al. "Novel Hyperelastic Models for Large Volumetric
% Deformations".
%
% The demo features:
% * Implementations of hyperelastic volumetric strain energy density
% functions (SEDs)
% * Visualizations of the SED, hydrostatic stress, and tangent as a
% function of the volume ratio.

%% Keywords
%
% * Strain energy density
% * Volumetric
% * Visualization

%%

clear; close all; clc;

%% Plot settings
fontSize=20;
fontSizeInner=fontSize+15;
fontSizeLabel=fontSize+30;
plotColors=gjet(4);
plotColors=plotColors([1 2 4],:);

lineWidth=3;
gridAlpha=0.3;
LineWidthAxis=2;
legendHeight=0.05;
numXTicks=5;

%% Control parameters

formulationCases=1:12;  %Choose formulation 1:12
k=1; %Default bulk modulus (except for hyperfoam)

J_max=2;
numPoints=2000; %Number of points for plotting
J=linspace(0.1,J_max,numPoints)'; %The volume ratios
xtickRange=linspace(0,max(J),numXTicks); %X-axis tick range

%% Get or set formulation specific data and parameters

for formulationCase=formulationCases

    switch formulationCase
        case 1 %Hencky
            formulationName='Hencky';
            parSet(1)=k; %Bulk modulus
        case 2 %Simo
            formulationName='Simo';
            parSet(1)=k; %Bulk modulus
        case 3 %Bischoff
            formulationName='Bischoff';
            b=2; %Beta
            parSet(1)=k; %Bulk modulus
            parSet(2)=b; %Beta
        case 4 %Modified Ogden
            formulationName='Modified Ogden';
            b=2;%Beta
            parSet(1)=k; %Bulk modulus
            parSet(2)=b; %Beta
        case 5 %Hyperfoam
            formulationName='Hyperfoam';
            mu=1;
            a=2; %Alpha
            b=2; %Beta
            parSet(1)=mu; %Mu
            parSet(2)=a; %Alpha
            parSet(3)=b; %Beta
        case 6 %Doll and Schweizerhoff
            formulationName='Doll and Schweizerhoff';
            a=3; %Alpha
            b=2; %Beta
            parSet(1)=k; %Bulk modulus
            parSet(2)=a; %Alpha
            parSet(3)=b; %Beta
        case 7 %Montella et al.
            formulationName='Montella et al.';
            k1=1;
            beta1=1/8;
            k2=1;
            beta2=1/8;
            m=4;
            parSet(1)=k; %Bulk modulus
            parSet(2)=beta1;
            parSet(3)=k2;
            parSet(4)=beta2;
            parSet(5)=m;
        case 8 %Moerman 1
            formulationName='Moerman 1';
            b1=3;
            b2=2;
            parSet(1)=k; %Bulk modulus
            parSet(2)=b1; %Alpha
            parSet(3)=b2; %Beta
        case 9 %Moerman 1A
            formulationName='Moerman 1A';
            b1=3;
            b2=2;
            q=0.5;
            parSet(1)=k; %Bulk modulus
            parSet(2)=b1; %Alpha
            parSet(3)=b2; %Beta
            parSet(4)=q; %Weigthing factor
        case 10 %Moerman 1B
            formulationName='Moerman 1B';
            b1=3;
            b2=2;
            parSet(1)=k; %Bulk modulus
            parSet(2)=b1; %Alpha
            parSet(3)=b2; %Beta
        case 11 %Moerman 2
            formulationName='Moerman 2';
            J1=max(J)+0.1;
            J2=0;
            parSet(1)=k; %Bulk modulus
            parSet(2)=J1; %J1
            parSet(3)=J2; %J2
        case 12 %Moerman 2A
            formulationName='Moerman 2A';
            b1=3;
            b2=2;
            parSet(1)=k; %Bulk modulus
            parSet(2)=b1; %Alpha
            parSet(3)=b2; %Beta
        case 13 %Moerman 3
            formulationName='Moerman 3';
            J1=max(J)+0.1;
            J2=0;
            s1=0.15;
            s2=0.15;
            q1=0.9;
            q2=0.9;
            parSet(1)=k; %Bulk modulus
            parSet(2)=J1; %J1
            parSet(3)=J2; %J2
            parSet(4)=s1; %s1
            parSet(5)=s2; %s2
            parSet(6)=q2; %q1
            parSet(7)=q2; %q2
    end

    %% Calculate SED

    %Get normalized SED
    [W,S,T]=SED_eval(formulationCase,parSet,J);

    %% Visualize data

    hf=cFigure;
    ht=subtitle(formulationName);
    ht.FontSize=fontSizeLabel;
    ht.Interpreter='latex';

    subplot(1,3,1); hold on;
    set(gca,'FontSize',fontSize,'LineWidth',LineWidthAxis,'GridAlpha',gridAlpha);
    xlabel('$J$','FontSize',fontSizeLabel,'Interpreter','latex');
    ylabel('$\Psi/\kappa$','FontSize',fontSizeLabel,'Interpreter','latex');

    hp1=plot(J,W,'k-','LineWidth',lineWidth);
    hp1.Color=plotColors(1,:);

    grid on; axis tight; axis square; box on;
    xlim([0 max(J(:))]);
    set(gca,'XTick',xtickRange);

    subplot(1,3,2); hold on;
    set(gca,'FontSize',fontSize,'LineWidth',LineWidthAxis,'GridAlpha',gridAlpha);
    xlabel('$J$','FontSize',fontSizeLabel,'Interpreter','Latex');
    ylabel('$\sigma_{h}/\kappa$','FontSize',fontSizeLabel,'Interpreter','Latex');
    grid on; axis tight; axis square; box on;

    hp2=plot(J,S,'k-','LineWidth',lineWidth);
    hp2.Color=plotColors(2,:);

    grid on; axis tight; axis square; box on;
    xlim([0 max(J(:))]);
    set(gca,'XTick',xtickRange);

    subplot(1,3,3); hold on;
    set(gca,'FontSize',fontSize,'LineWidth',LineWidthAxis,'GridAlpha',gridAlpha);
    xlabel('$J$','FontSize',fontSizeLabel,'Interpreter','latex');
    ylabel('$\frac{\partial^2 \Psi}{\partial J^2} /\kappa$','FontSize',fontSizeLabel,'Interpreter','Latex');

    hp3=plot(J,T,'k-','LineWidth',lineWidth);
    hp3.Color=plotColors(3,:);

    grid on; axis tight; axis square; box on;
    xlim([0 max(J(:))]);
    ylim([0 max(T(:))]);
    set(gca,'XTick',xtickRange);

    drawnow;

end

%% Evaluate SED

function [W,S,T]=SED_eval(formulationCase,parSet,J)

switch formulationCase
    case 1 %Hencky
        k=parSet(1);
        W=k/2*log(J).^2;
        S=k*log(J)./J;
        T=(k-k*log(J))./J.^2;
    case 2 %Simo
        k=parSet(1);
        W=(k/2).*(J-1).^2;
        S=(k/2).*(2*J-2);
        T=k*ones(size(J));
    case 3 %Bischoff
        k=parSet(1);
        b=parSet(2);
        W=(k./b.^2).*(cosh(b*(J-1))-1);
        S=(k./b)   .* sinh(b*(J-1));
        T=k           .* cosh(b*(J-1));
    case 4 %Modified Ogden
        k=parSet(1);
        b=parSet(2);
        W=(k./b.^2).*(J.^-b - 1 + b.*log(J));
        S=(k./b)   .*(1./J     - J.^(-b-1));
        T=(k./b)   .*(-1./J.^2  + (b+1).*J.^(-b-2));
    case 5 %Hyperfoam
        mu=parSet(1);
        a=parSet(2);
        b=parSet(3);
        k=mu.*(b+1/3);
        W=(2*mu./(a.^2)).*( 3*(J.^(a/3)-1)...
            +(1./b.*( (J.^(-a.*b))-1 )) );
        S=(1./J).*(2*mu./a).*(J.^(a/3)...
            -J.^(-a.*b) );
        T=(1./(J.^2)).*(2*mu/a).*((a./3-1).*J.^(a./3) +...
            (a.*b+1).*J.^(-a*b) );
    case 6 %Doll and Schweizerhoff
        k=parSet(1);
        a=parSet(2);
        b=parSet(3);
        W=( (k/(a+b)).*( ((1/(a+1)).*(J.^(a+1))) + ((1/(b-1)).*(J.^(-b+1)))) )...
            -(k.*(1/(a+1)).*(1/(b-1)));
        S=(k/(a+b)).*(J.^a-J.^(-b));
        T=(k/(a+b)).*(a*J.^(a-1)+b*J.^(-b-1));
    case 7
        k=parSet(1);
        beta1=parSet(2);
        k2=parSet(3);
        beta2=parSet(4);
        m=parSet(5);

        %SED
        W1=k./(2.*beta1).*exp(beta1.*   (log(J).^2))-(k/(2*beta1));
        W2=k2./(m.*beta2).*exp(beta2.*abs(log(J).^m))-(k2/(m*beta2));
        W=W1+W2;

        %Stress
        S1=(k./J).*(exp(beta1.*    log(J).^2) .*     log(J));
        S2=(k2./J).*(exp(beta2.*abs(log(J)).^m).*(abs(log(J)).^(m-1)).*sign(log(J)));
        S=S1+S2;

        %Tangent
        T1=(k./J.^2) .* exp(beta1.*    log(J) .^2).*(beta1.*2.*    log(J) .^2 - log(J) + 1);
        T2=(k2./J.^2) .* exp(beta2.*abs(log(J)).^m).*(beta2.*m.*abs(log(J)).^m - log(J) + m -1).*abs(log(J)).^(m-2);
        T=T1+T2;

    case 8 %Moerman 1
        k=parSet(1);
        b1=parSet(2);
        b2=parSet(3);
        W=(k/4)      .*( (1/b1^2).*((J.^ b1)-1).^2 + ...
            (1/b2^2).*((J.^-b2)-1).^2 );
        S=(k/2)./J   .*( (1/b1  ).*(J.^( 2*b1) - J.^b1 ) - ...
            (1/b2  ).*(J.^(-2*b2) - J.^-b2) );
        T=(k/2)./J.^2.*( ((2-1/b1)*J.^( 2*b1)-(1-1/b1)*J.^b1) + ...
            ((2+1/b2)*J.^(-2*b2)-(1+1/b2)*J.^-b2) );
    case 9 %Moerman 1A
        k=parSet(1);
        b1=parSet(2);
        b2=parSet(3);
        q=parSet(4);

        W1=(k/(2*b1^2)).*(  q).*((J.^ b1)-1).^2;
        W2=(k/(2*b2^2)).*(1-q).*((J.^-b2)-1).^2;
        W=W1+W2;

        S1= (k/b1)./J.*(  q).*(J.^( 2*b1)-J.^b1 );
        S2=-(k/b2)./J.*(1-q).*(J.^(-2*b2)-J.^-b2);
        S=S1+S2;

        T1=(k./J.^2).*(  q).*((2-1/b1)*J.^( 2*b1)-(1-1/b1)*J.^b1 );
        T2=(k./J.^2).*(1-q).*((2+1/b2)*J.^(-2*b2)-(1+1/b2)*J.^-b2);
        T=T1+T2;
    case 10 %Moerman 1B
        k=parSet(1);
        b1=parSet(2);
        b2=parSet(3);

        L1=(J>=1);
        L2=(J<1);

        W=zeros(size(J));
        W(L1)=(k/(2*b1^2)).*((J(L1).^ b1)-1).^2;
        W(L2)=(k/(2*b2^2)).*((J(L2).^-b2)-1).^2;

        S=zeros(size(J));
        S(L1)= (k/b1).*(J(L1).^( 2*b1-1)-J(L1).^( b1-1));
        S(L2)=-(k/b2).*(J(L2).^(-2*b2-1)-J(L2).^(-b2-1));

        T=zeros(size(J));
        T(L1)=(k/b1).*((2*b1-1)*J(L1).^( 2*b1-2)-(b1-1)*J(L1).^( b1-2));
        T(L2)=(k/b2).*((2*b2+1)*J(L2).^(-2*b2-2)-(b2+1)*J(L2).^(-b2-2));
    case 11 %Moerman 2
        k=parSet(1);
        J1=parSet(2);
        J2=parSet(3);

        a1=(2/pi)*(J1-1);
        a2=(2/pi)*(J2-1);

        W1=(-k*a1.^2)*log(cos((J-1)/a1));
        W2=(-k*a2.^2)*log(cos((J-1)/a2));

        W=zeros(size(W1));
        W(J>=1)=W1(J>=1);
        W(J<1)=W2(J<1);
        W=real(W);
        W(J>=J1)=inf;
        W(J<=J2)=inf;

        S1=(k.*a1).*tan((J-1)/a1);
        S2=(k.*a2).*tan((J-1)/a2);
        S=zeros(size(S1));
        S(J>=1)=S1(J>=1);
        S(J<1)=S2(J<1);
        S(J>=J1)=inf;
        S(J<=J2)=-inf;

        T1=k*sec((J-1)/a1).^2;
        T2=k*sec((J-1)/a2).^2;
        T=zeros(size(T1));
        T(J>=1)=T1(J>=1);
        T(J<1)=T2(J<1);
        T(J>=J1)=inf;
        T(J<=J2)=inf;
    case 12 %Moerman 2A
        k=parSet(1);
        b1=parSet(2);
        b2=parSet(3);

        W1=(k./b1.^2).*(cosh(b1*(J-1))-1);
        S1=(k./b1)   .* sinh(b1*(J-1));
        T1=k            .* cosh(b1*(J-1));

        W2=(k./b2.^2).*(cosh(b2*(J-1))-1);
        S2=(k./b2)   .* sinh(b2*(J-1));
        T2=k            .* cosh(b2*(J-1));

        W3=k.*(-4/pi^2) .*log(cos(pi/2*(1-J)));
        S3=k.*(-2/pi)   .*tan(pi/2*(1-J));
        T3=k            .*sec(pi/2*(1-J)).^2;

        W=W1;
        S=S1;
        T=T1;

        W(J<1)=W2(J<1)/2+W3(J<1)/2;
        S(J<1)=S2(J<1)/2+S3(J<1)/2;
        T(J<1)=T2(J<1)/2+T3(J<1)/2;
    case 13 %Moerman 3
        k=parSet(1);
        J1=parSet(2);
        J2=parSet(3);
        s_1=parSet(4);
        s_2=parSet(5);
        q1=parSet(6);
        q2=parSet(7);

        L=J<1;

        % PART 1
        a1=(pi/2)*(1/(J1-1));
        a2=(pi/2)*(1/(J2-1));

        %SED
        W11=(-1/a1.^2)*log(cos((J-1).*a1));
        W21=(-1/a2.^2)*log(cos((J-1).*a2));
        W1=zeros(size(J));
        W1(~L)=W11(~L);
        W1(L)=W21(L);
        W1=real(W1);
        W1(J>J1)=inf;
        W1(J<J2)=inf;

        %Stress
        S11=(1./a1).*tan((J-1).*a1);
        S21=(1./a2).*tan((J-1).*a2);
        S1=zeros(size(J));
        S1(~L)=S11(~L);
        S1(L)=S21(L);
        S1(J>J1)=inf;
        S1(J<J2)=-inf;

        %Tangent
        T11=sec((J-1).*a1).^2;
        T21=sec((J-1).*a2).^2;
        T1=zeros(size(J));
        T1(~L)=T11(~L);
        T1(L)=T21(L);
        T1(J>J1)=inf;
        T1(J<J2)=inf;

        % PART 2
        b1= k/(s_1);
        b2= k/(s_2);

        W12=(1/b1.^2)*log(cosh((J-1).*b1));
        W22=(1/b2.^2)*log(cosh((J-1).*b2));
        W2=zeros(size(J));
        W2(~L)=W12(~L);
        W2(L)=W22(L);
        W2=real(W2);

        S12=1/b1*tanh((J-1)*b1);
        S22=1/b2*tanh((J-1)*b2);
        S2=zeros(size(J));
        S2(~L)=S12(~L);
        S2(L)=S22(L);

        T12=sech((J-1).*b1).^2;
        T22=sech((J-1).*b2).^2;
        T2=zeros(size(J));
        T2(~L)=T12(~L);
        T2(L)=T22(L);

        % SUM

        W(~L)=k*((1-q1)*W1(~L)+q1*W2(~L));
        S(~L)=k*((1-q1)*S1(~L)+q1*S2(~L));
        T(~L)=k*((1-q1)*T1(~L)+q1*T2(~L));

        W(L)=k*((1-q2)*W1(L)+q2*W2(L));
        S(L)=k*((1-q2)*S1(L)+q2*S2(L));
        T(L)=k*((1-q2)*T1(L)+q2*T2(L));

        %%

        W(J>=J1)=inf;
        W(J<=J2)=inf;

        S(J>=J1)=inf;
        S(J<=J2)=-inf;

        T(J>=J1)=inf;
        T(J<=J2)=inf;

end

%Normalise based on bulk-modulus
W=W/k; S=S/k; T=T/k;

end

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

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


% Bezier parameters
% Copyright by Nguyen Quoc Duan - EMMC11

% Purpose : to compute the parameter terms in Bezier curves
% n : the polynomial degree of Bezier curve

function B_para=B_para(i,n,t)
B_para=(1-t)^(n-i)*t^i;


function [laglength, AIC, logL] = VARlag(ENDO,maxlag,const,EXOG,lag_ex)
% =======================================================================
% Determine VAR lag length wit Akaike criterion
% =======================================================================
% [laglength, AIC, logL] = VARlag(ENDO,maxlag,const,EXOG,lag_ex)
% -----------------------------------------------------------------------
% INPUT
%   - ENDO: an (nobs x nvar) matrix of endogenous variables.
%	- maxlag: the maximum lag length over which Akaike information criterion
%       is computed
% -----------------------------------------------------------------------
% OPTIONAL INPUT
%   - const: 0 no const; 1 const ; 2 const&trend; 3 const&trend^2; (dflt = 1)
%	- EXOG: optional matrix of variables (nobs x nvar_ex)
%   - nlag_ex: number of lags for exogeonus variables (dflt = 0)
% -----------------------------------------------------------------------
% OUTPUT
%	- laglength: preferred lag lenghth according to Akaike information criterion
%   - AIC: vector [maxlag x 1] of Akaike information criterion
%   - logL: vector [maxlag x 1] of loglikelihood
% =======================================================================
% Ambrogio Cesa Bianchi, March 2015
% ambrogio.cesabianchi@gmail.com

% Note:
% The determinant of the residual covariance is computed without adjusting
% for the degrees of freedom. The log likelihood value is computed assuming
% a multivariate normal (Gaussian) distribution

%% Check inputs
%=========================================================
[nobs, ~] = size(ENDO);

% Check if ther are constant, trend, both, or none
if ~exist('const','var')
    const = 1;
end

% Check if there are exogenous variables
if exist('EXOG','var')
    [nobs2, num_ex] = size(EXOG);
    % Check that ENDO and EXOG are conformable
    if (nobs2 ~= nobs)
        error('var: nobs in EXOG-matrix not the same as y-matrix');
    end
    clear nobs2
else
    num_ex = 0;
end

% Check if there is lag order of EXOG, otherwise set it to 0
if ~exist('lag_ex','var')
    lag_ex = 0;
end

% number of exogenous variables per equation
nvar_ex = num_ex*(lag_ex+1);

%% Compute log likelihood and Akaike criterion
%=========================================================
logL = zeros(maxlag,1);
AIC  = zeros(maxlag,1);
for i=1:maxlag
    X = ENDO(maxlag+1-i:end,:);
    aux = VARmodel(X,i,const);
    if nvar_ex>0
        Y = EXOG(maxlag+1-i:end,:);
        aux = VARmodel(X,i,const,Y,lag_ex);
    end
    NOBS = aux.nobs;
    NEQS = aux.nvar;
    NTOTCOEFF = aux.ntotcoeff;
    RES = aux.residuals;
    SIGMA = (1/(NOBS)).*(RES)'*(RES); %use non dof adjusted version of sigma as in Eviews
    logL(i) = -(NOBS/2)* (NEQS*(1+log(2*pi)) + log(det(SIGMA)));
    AIC(i)  = -2*(logL(i)/NOBS) + 2*(NEQS*NTOTCOEFF/NOBS);
end

laglength = find(AIC==min(AIC));


function [Au, PHIu, Av, PHIv, w, TWOCIR]=ep2ap(SEMA, ECC, INC, PHA, plot_demo)
%
% Convert tidal ellipse parameters into amplitude and phase lag parameters.
% Its inverse is app2ep.m. Please refer to app2ep for the meaning of the
% inputs and outputs.
%
% Zhigang Xu
% Oct. 20, 2000
%
% Document:  tidal_ellipse.ps
%
if nargin < 5
     plot_demo=0;  % by default, no plot for the ellipse
end

   Wp = (1+ECC)/2 .*SEMA;
   Wm = (1-ECC)/2 .*SEMA;
   THETAp = INC-PHA;
   THETAm = INC+PHA;

   %convert degrees into radians
   THETAp = THETAp/180*pi;
   THETAm = THETAm/180*pi;

   %Calculate wp and wm.
   wp = Wp.*exp(i*THETAp);
   wm = Wm.*exp(i*THETAm);

   if nargout >= 5
      ndot=36;
      dot = 2*pi/ndot;
      ot = [0:dot:2*pi-dot];
      w = wp(:)*exp(i*ot)+wm(:)*exp(-i*ot);
      w=reshape(w, [size(wp) ndot]);
   end

   % Calculate cAu, cAv --- complex amplitude of u and v
   cAu = wp+conj(wm);
   cAv = -i*(wp-conj(wm));
   Au  = abs(cAu);
   Av  = abs(cAv);
   PHIu = -angle(cAu)*180/pi;
   PHIv = -angle(cAv)*180/pi;

   % flip angles in the range of [-180 0) to the range of [180 360).
   id = PHIu < 0; PHIu(id) = PHIu(id) + 360;
   id = PHIv < 0; PHIv(id) = PHIv(id) + 360;

  if any(plot_demo)
     plot_ell(SEMA,ECC,INC,PHA,plot_demo);
  end

  if nargout == 6
      TWOCIR=struct('Wp', Wp, 'THETAp', THETAp, 'wp', ...
            wp, 'Wm', Wm, 'THETAm', THETAm, 'wm', wm, 'ot', ot, 'dot', dot);
 end

%Authorship Copyright:
%
%    The author of this program retains the copyright of this program, while
% you are welcome to use and distribute this program as long as you credit
% the author properly and respect the program name itself. Particularly,
% you are expected to retain the original author's name in this original
% version of the program or any of its modified version that you might make.
% You are also expected not to essentially change the name of the programs
% except for adding possible extension for your own version you might create,
% e.g. app2ep_xx is acceptable.  Any suggestions are welcome and enjoy my
% program(s)!
%
%
%Author Info:
%_______________________________________________________________________
%  Zhigang Xu, Ph.D.
%  (pronounced as Tsi Gahng Hsu)
%  Research Scientist
%  Coastal Circulation
%  Bedford Institute of Oceanography
%  1 Challenge Dr.
%  P.O. Box 1006                    Phone  (902) 426-2307 (o)
%  Dartmouth, Nova Scotia           Fax    (902) 426-7827
%  CANADA B2Y 4A2                   email xuz@dfo-mpo.gc.ca
%_______________________________________________________________________
%
%Release Date: Nov. 2000


function x = gendatav(pc,R0,nobs)
%function x = gendatav(pc,R0,nobs)
%
%  Simulates nobs observations of a vector AR-process
%  with partial correlations pc and covariance matrix R0.
%
%  See also FILTERV, RANDN.

%  S. de Waele, FEB 2001.

if ~isstatv(pc), error('Partial correlations non-stationairy!'), end
order = kingsize(pc,3)-1;
dim = size(pc,1);
I = eye(dim);
[rc,rcb] = pc2rcv(pc,R0);
[Pf,Pb] = pc2resv(pc,R0);
par = zeros(dim,dim,order+1);
parb = zeros(dim,dim,order+1);

x = zeros(dim,1,nobs); %The signal in matrix notation
%First observation
innovation = randncov(Pf(:,:,1));
x(:,:,1) = innovation;

par(:,:,1) = I;
parb(:,:,1)= I;
if order,
	par(:,:,2) = rc(:,:,2);
	parb(:,:,2)= rcb(:,:,2);
	par_o  = par;
	parb_o = parb;
end

%observation 2 to order
for obs = 2:order,
   innovation = randncov(Pf(:,:,obs));
   prediction = -prodsumv(flipdim(par(:,:,2:obs),3),x(:,:,1:obs-1));
   x(:,:,obs) = prediction+innovation;

   p=obs; %The next observations requires the AR(obs)-model
   %par(:,:,2:p) =  par_o(:,:,2:p) +flipdim(filterv(rc(:,:,p+1),1,parb_o(:,:,2:p)),3);
   par(:,:,2:p) =  par_o(:,:,2:p) +flipdim(armafilterv(parb_o(:,:,2:p),1,rc(:,:,p+1)),3);
   par(:,:,p+1)= rc(:,:,p+1);
   parb(:,:,2:p) =  parb_o(:,:,2:p) +flipdim(armafilterv(par_o(:,:,2:p),1,rcb(:,:,p+1)),3);
   parb(:,:,p+1)= rcb(:,:,p+1);

   par_o  = par;
   parb_o = parb;

end %for obs = 2:order,

%observations order+1 to nobs
innovation = zeros(dim,1,nobs-order);
for i = 1:nobs-order,
   innovation(:,:,i) = randncov(Pf(:,:,order+1));
end
x(:,:,order+1:end) = armafilterv(innovation,par,I,x(:,:,1:order));


function [sims_with_endopath,EPSn] = cforecasts(endo_path,endo_index,forecast_data,Phi,Sigma)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 'cforecasts' performs the forecast conditional on a path for one or more
% endogenous variables using all the shocks of the VAR
% references: Waggoner and Zha (1999) and Jarocinski (2010)

% Inputs:
% - endo_path, path for the endogenous variables
% - endo_index, order of the endogenous variables with a contional path
% - forecast_data, last data
% - Phi, AR parameters of the VAR
% - Sigma, Covariance matrix of the reduced form VAR shocks

% Output:
% - sims_with_endopath
% 1st dimension:   horizon
% 2nd dimension:   variable
% - EPSn, shocks generating the path

% Filippo Ferroni, 6/1/2015
% Revised, 2/15/2017
% Revised, 3/21/2018
% Revised, 9/11/2019
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[fhor,Ncondvar] = size(endo_path);
Nvar            = size(Sigma,2);

if size(Ncondvar,2) ~= length(endo_index)
    error('something went wrong: the number of path and the number of vars are not consistent.')
end
if size(endo_index,1) > size(endo_index,2)
    error('the index needs to be a row vector');
end
Nres = size(endo_index,2)*fhor;

[F,~,~,const,~,lags]     = var2ss(Phi,Sigma);

% endo_path_deviation = endo_path - repmat(const',1,fhor);

% no shocks forecasts
[sims_no_shock,~]   = forecasts(forecast_data,Phi,Sigma,fhor,lags);

% restrictions
% err                 = endo_path_deviation - sims_no_shock(:,endo_path_index);
% err = zeros(Nvar*fhor);
err = endo_path - sims_no_shock(:,endo_index);
err = reshape(err',size(endo_index,1)*fhor,1);

% 1 standard deviation increase (if unit = eye(N))
[C] = chol(Sigma,'lower');

R    = zeros(Nvar * fhor);
tmp0 = [];
for ff = 1 :fhor
    tmp  = F^(ff-1);
    tmp0 = [tmp(1:Nvar,1:Nvar) * C tmp0];
    R((ff-1)*Nvar + 1 : (ff)*Nvar, 1: size(tmp0,2) ) = tmp0;
    index(ff,:) = (ff-1)*Nvar + endo_index;
end
index = reshape(index',Nres,1);
% index= [4 10];
Rtilde = R(index,:);

[U,D,V] = svd(Rtilde);
V1 = V(:,1:Nres);
V2 = V(:,Nres+1:end);

eps = V1*inv(D(1:Nres,1:Nres))*U'*err + V2 * randn(size(R,1)-Nres,1);

% orthogonal schoks
EPSn = reshape(eps,Nvar,fhor);
% reduced form shocks
EPS  = C*EPSn;

[~,sims_with_endopath] = forecasts(forecast_data,Phi,Sigma,fhor,lags,EPS');


% SOSDEMO6 --- MAX CUT
% Section 3.6 of SOSTOOLS User's Manual
%

clear; echo on;
syms x1 x2 x3 x4 x5;
vartable = [x1; x2; x3; x4; x5];

% Number of cuts
f = 2.5 - 0.5*x1*x2 - 0.5*x2*x3 - 0.5*x3*x4 - 0.5*x4*x5 - 0.5*x5*x1;

% Boolean constraints
bc{1} = x1^2 - 1 ;
bc{2} = x2^2 - 1 ;
bc{3} = x3^2 - 1 ;
bc{4} = x4^2 - 1 ;
bc{5} = x5^2 - 1 ;

% =============================================
% First, initialize the sum of squares program
prog = sosprogram(vartable);

% =============================================
% Then define SOSP variables

% -- p1(x) -- : sum of squares
% Monomial vector: 5 independent variables, degree <= 1
Z = monomials(vartable,[0 1]);
[prog,p{1}] = sossosvar(prog,Z);

% -- p2(x) ... p6(x) : polynomials
% Monomial vector: 5 independent variables, degree <= 2
Z = monomials(vartable,0:2);
for i = 1:5
    [prog,p{1+i}] = sospolyvar(prog,Z);
end;

% =============================================
% Next, define SOSP constraints

% Constraint : p1(x)*(gamma - f(x)) +  p2(x)*bc1(x)
%               + ... + p6(x)*bc5(x) - (gamma-f(x))^2 >= 0
gamma = 4;

expr = p{1}*(gamma-f);
for i = 2:6
    expr = expr + p{i}*bc{i-1};
end;
expr = expr - (gamma-f)^2;

prog = sosineq(prog,expr);

% =============================================
% And call solver
prog = sossolve(prog);

% =============================================
% Program is feasible, thus 4 is an upper bound for the cut.
echo off


function [interpolated_values] = gsp_pwl_warp_fn(x,y,x0)

cut=1e-4;

if max(x0)>max(x)+cut
    error('GSP_PWL_WARP_FN: This function does not allow you to interpolate ousite the point x and y')
end

if min(x0)<min(x)-cut
    error('GSP_PWL_WARP_FN: This function does not allow you to interpolate ousite the point x and y')
end

sx0 = size(x0);
x0 = x0(:);

% Make sure data is sorted and monotonic
[x,x_ind]=sort(x,'ascend');
y=y(x_ind);
if ( isequal(sort(y,'ascend'),y)==0 && isequal(sort(y,'descend'),y)==0 )
    error('Data points are not monotonic');
end

% Piecewise-linear interpolation
num_pts=length(x);
num_pts_to_interpolate=length(x0);
interpolated_values=zeros(num_pts_to_interpolate,1);

for i=1:num_pts_to_interpolate
    [~,closest_ind]=min(abs(x-x0(i)));
    if (x(closest_ind)-x0(i))<(-cut) || ( abs(x(closest_ind)-x0(i))<cut && closest_ind < num_pts)
    %if sign(x(closest_ind)-x0(i))<0 || ( sign(x(closest_ind)-x0(i))==0 && closest_ind < num_pts)
        lower_ind=closest_ind;
    else
        lower_ind=closest_ind-1;
    end
    interpolated_values(i)=y(lower_ind)*(x(lower_ind+1)-x0(i))/(x(lower_ind+1)-x(lower_ind)) + y(lower_ind+1)*(x0(i)-x(lower_ind))/(x(lower_ind+1)-x(lower_ind));
end

interpolated_values = reshape(interpolated_values,sx0);

end

% Old Code:
% function [output] = gsp_pwl_warp_fn(approx_spectrum,x0)
% N=length(approx_spectrum);
% output=zeros(size(x0));
% for i=1:length(x0);
%     left_index=sum(approx_spectrum<=x0(i));
%     left_val=approx_spectrum(max(left_index,1));
%     right_val=approx_spectrum(min(left_index+1,N));
%     output(i)=left_index+(x0(i)-left_val)/(right_val-left_val);
% end
%
% end


function ppwd = pwd_imp_circexp(pm,Npw)
%PWD_IMP_CIRCEXP converts a circular basis expansion of a sound field to its
%two-dimensional plane wave decomposition
%
%   Usage: ppwd = pwd_imp_circexp(pm,[Npw])
%
%   Input parameters:
%       pm      - circular basis expansion [N x (M+1)]
%       Npw     - number of equi-angular distributed plane waves, optional,
%                 default: 2*M+1
%
%   Output parameters:
%       ppwd    - plane wave decomposition [N x Npw]
%
%   See also: driving_function_imp_localwfs_sbl_ps,
%   driving_function_imp_localwfs_sbl_pw

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

%% ===== Checking of input parameters ==================================
nargmin = 1;
nargmax = 2;
narginchk(nargmin,nargmax);
isargmatrix(pm);
M = size(pm,2)-1;
if nargin == nargmin
  Npw = 2*M+1;
else
  isargpositivescalar(Npw);
end

%% ===== Computation ====================================================
% Implementation of
%                 ___
% _               \
% p(phipw, t) =   /__     p (t) i^m  e^(-i m phipw)
%               m=-M..M    m
% with
%
% phipw = n * 2*pi/Npw

pm = [conj(pm(:,end:-1:2)), pm];  % append coefficients for negative m
ppwd = inverse_cht(bsxfun(@times,pm,1i.^(-M:M)),Npw);


%MAKEFUZZYEDGESYS Script to make MAT-file used by FUZZYFILT.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Input membership functions.
zero = @(z) bellmf(z,-0.3,0);
not_used = @(z) onemf(z);

% Output membership functions.
black = @(z) triangmf(z,0,0,0.75);
white = @(z) triangmf(z,0.25,1,1);

% There are four rules and four inputs, so the inmf matrix is 4x4. Each
% row of the inmf matrix corresponds to one rule.
inmf = {zero, not_used, zero, not_used
   not_used, not_used, zero, zero
   not_used, zero, not_used, zero
   zero, zero, not_used, not_used};

% Specify cell array of output membership functions, OUTMF (see function
% IMPLFCNS). There are four IF-THEN rules in this problem, all resulting
% in WHITE, and one ELSE rule resulting in BLACK (see Fig. 3.40).
outmf = {white,white,white,white,black};

% Inputs to the output membership functions are in the range [0,1].
vrange = [0,1];

F = fuzzysysfcn(inmf,outmf,vrange);

% Compute a lookup-table-based approximation to the fuzzy system
% function. Each of the four inputs is in the range [-1,1].
G = approxfcn(F,[-1 1; -1 1; -1 1; -1 1]);

% Save the fuzzy system approximation function to a MAT-file.
save fuzzyedgesys G


function Fmat = extForce_gen(tsave, start_T, duration, mag, direction)

% Direction vector of the external force (unit vector)
dir_F = direction/norm(direction);
F = mag * dir_F;
Fmat = zeros(length(tsave),3);

% time frame : tsave - duration - tsave

for i = 1:length(tsave)
    if tsave(i) >= start_T && tsave(i) < start_T + duration/6
        Fmat(i,:) = F*(sin(2*pi/(duration/3)*(tsave(i)-start_T)-pi/2) + 1)/2;

    elseif tsave(i) >= start_T + duration/6 && tsave(i) < start_T + duration*5/6
        Fmat(i,:) = F;

    elseif tsave(i) >= start_T + duration*5/6 && tsave(i) < start_T + duration
        Fmat(i,:) = F*(sin(2*pi/(duration/3)*(tsave(i)-(start_T+duration*5/6)) + pi/2) + 1)/2;
    end
end
Fmat = [tsave', Fmat];

end


function [rs, archetypes] = arch(s, na, mode)

%tstoolbox/@signal/arch
%   Syntax:
%     * [rs, archetypes]=arch(s, na, mode='normalized')
%
%   Input arguments:
%     * na - number of generated archetypes
%     * mode - mode can be one of the following : 'normalized' , 'mean',
%       'raw' (optional)
%
%   Archetypal analysis of column orientated data set:
%     * each row of data is one 'observation', e.g. the sample values of
%       all channels in a multichannel measurement at one point in time
%     * in mode 'normalized' each column of data is centered by removing
%       its mean and then normalized by dividing through its standard
%       deviation before the covariance matrix is calculated
%     * in mode 'mean' only the mean of every column of data is removed
%     * in mode 'raw' no preprocessing is applied to data
%
%   Default value for mode is 'normalized'.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

narginchk(2,3);

if nargin < 3
	mode = 'normalized';
end

if ndim(s)~=2
	error('arch needs a signal with two dimensions as input')
end

dat = data(s);
n = dlens(s,1);
m = dlens(s,2);

htext = {''};
htext{end+1} = 'Applied Archetypal Analysis (arch)';

[Z,A,B] = arch2(data(s)', na, mode)

a = achse(unit, 1,1);
a = setname(a, 'Archetyp');

rs = signal(core(A'), s);
rs = addhistory(rs, htext);
rs = addcommandlines(rs, 's = arch(s', na, mode);
rs = setaxis(rs, 2, a);
rs = setplothint(rs, 'multigraph');

archetypes = signal(core(Z),s);
archetypes = addhistory(archetypes, htext);
archetypes = addcommandlines(archetypes, '[dummy, s] = arch(s', na, mode);
archetypes = setaxis(archetypes, 2, a);
archetypes = setaxis(archetypes, 1, getaxis(s,2));


function [t,p1,p2] = intersectSegments(l1,l2)

% INTERSECTSEGMENTS  Intersect two 3d segments.
%   [T,P1,P2] = INTERSECTSEGMENTS(L1,L2) intersects the two segments
%   L1=[E1,F1] and L2=[E2,F2], and returns a vector of abscissas T=[T1;T2],
%   and two intersection points P1 and P2, so that
%
%       P1 = E1 + T1*V1
%       P2 = E2 + T2*V2
%
%   where V1=F1-E1 and V2=F2-E2 are director vectors.
%
%   Note that what we really intersect is the infinite line supporting the
%   segments. Some examples (intersection is at the cross '+'):
%
%       ----+----         +         |
%                                   |
%           |           / |     ----+---
%           |          /  |         |
%
%   If the lines do intersect in the 3D space, we have the intersection
%   point P = P1 = P2. Otherwise, P1 and P2 are the points of the lines
%   that are closest to the other line, that is, the line passing over P1
%   and P2 is orthogonal to both L1 and L2.
%
%   See also INTERSECTPVLINES, SEG2PVLIN.

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

m1 = seg2pvLin(l1);
m2 = seg2pvLin(l2);

[t,p1,p2] = intersectPvLines(m1,m2);

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


function [ vX, mX ] = SolveLsL1ComplexCd( mA, vB, lambdaFctr, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX, mX ] = SolveLsL1ComplexPgm( mA, vB, lambdaFctr, numIterations )
% Solves the 0.5 * || A x - b ||_2 + \lambda || x ||_1 problem using
% Coordinate Descent Method. The model allows A, b and x to be Complex.
% Input:
%   - mA                -   Model Matrix.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double' (Complex).
%                           Range: (-inf, inf).
%   - vB                -   Input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double' (Complex).
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L1 Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia Coordinate Descent Method - https://en.wikipedia.org/wiki/Coordinate_descent.
% Remarks:
%   1.  Coordienat Descent is basically Steepest Descnt in L1 Norm.
% Known Issues:
%   1.  A
% TODO:
%   1.  B
% Release Notes:
%   -   1.0.000     07/11/2016
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

vANorm = sum(mA .* conj(mA), 1);
numElements = size(mA, 2); %<! Size of solution

vX  = pinv(mA) * vB; %<! Dealing with "Fat Matrix"

mX = zeros([size(vX, 1), numIterations]);
mX(:, 1) = vX;

for ii = 2:numIterations

    for jj = 1:numElements
         vA = mA(:, jj);
         colNormSqr = vANorm(jj);
         vExcCoord = [1:jj - 1, jj + 1:numElements];

         vR = vB - (mA(:, vExcCoord) * vX(vExcCoord));
         vBeta = vA' * vR;

         vX(jj) = ProxL1(vBeta, lambdaFctr) / colNormSqr;
    end

    mX(:, ii) = vX;

end

end

function [ vX ] = ProxL1( vX, lambdaFactor )

% Soft Thresholding - Complex Domain -> Keep Phase, Soft Threshold the
% Modulus

% vXAbs   = abs(vX);
% vXPhase = angle(vX);
%
% vX = max(vXAbs - lambdaFactor, 0) .* exp(1i * vXPhase);

vXAbs = abs(vX);

vX              = (vX ./ vXAbs) .* max((vXAbs - lambdaFactor), 0);
vX(vXAbs == 0)  = 0;

end


function M = compute_compressed_sensing_matrix(p,n, type, options)

% compute_compressed_sensing_matrix - compute a CS matrix
%
%   M = compute_compressed_sensing_matrix(p,n, type, options);
%
%   M is a (p,n) matrix.
%   type can be
%       'gaussian': gaussian entries
%       'randnormed': random unit norm columns
%       'bernouilli': random +-1/sqrt(p)
%       'orthoproj': random projector M'*M=Id
%       'fourier': random Fourier rows
%       'sincos': random cosine/sine rows
%
%   Copyright (c) 2008 Gabriel Peyre

if nargin<3
    type = 'gaussian';
end

options.null = 0;
switch lower(type)
    case 'gaussian'
    	M = randn(p,n) / sqrt(p);
    case {'randnormed' 'rand'}
        M = randn(p,n);
        M = M ./ repmat( sqrt(sum(M.^2)), [p 1] );
    case 'bernouilli'
        M = sign(randn(p,n)) / sqrt(p);
    case {'orthoproj' 'randproj'}
        M = randn(p,n);
        [M,R] = qr(M'); M = M(:,1:p)';
    case 'fourier'
        [X,Y] = meshgrid(0:n-1:0:n-1);
        M = exp( 2i*pi/n * X.*Y ) / sqrt(p);
        sel = randperm(n); sel = sel(1:p);
        M = M(sel,:);
    case 'sincos'
        [X,Y] = meshgrid(0:n-1,0:n/2);
        M = cos( 2*pi/n * X.*Y );
        [X,Y] = meshgrid(0:n-1,1:n/2-1);
        M = [M; sin( 2*pi/n * X.*Y )];
        M = M ./ repmat( sqrt(sum(M.^2,2)), [1 n] );
        sel = randperm(n); sel = sel(1:p);
        M = M(sel,:) * sqrt( n/p );
    case 'projoptim'
        niter = getoptions(options, 'niter_projoptim', 40);
        M = randn(p,n);
        for i=1:niter
            % unit normed
            M = M ./ repmat( sqrt(sum(M.^2)), [p 1] );
            % projector
            [U,S,V] = svd(M, 'econ');
            S = diag(diag(S)*0+1);
            M = U*S*V';
        end
    otherwise
        error('Unknown matrice type');
end

return;
%% old code

if nargin<4
    normtype = 'normalize';
end

switch lower(type)
    case 'random'
        M = randn(n,m);
    case 'bumps'
        normtype = 'none';
        % m bumps
        sigma = 0.05;
        x = linspace(0,1,m+1)'; x(end) = []; % bump centers
        M = zeros(n,m);
        t = linspace(0,1,n+1)'; t(end) = [];
        for i=1:m
            u = t-x(i); u(u>1/2) = 1-u(u>1/2);
            M(:,i) = exp( -u.^2 / (2*sigma^2)  );
        end
end

switch lower(normtype)
    case 'normalize'
        % lines should be of unit norm
        d = sqrt( sum(M.^2,1) );
        M = M ./ repmat( d, [size(M,1) 1] );
    case 'orthogonalize'
        % M*M' = Id
        M = orth(M')';
    case 'none'
    otherwise
        error('Unknown normalization');
end


function [xi,w]=fifthOrder2DCubPoints()
%%FIFTHORDER2DCUBPOINTS Generate fifth-order cubature points
%               for integration over a 2-dimensional cube with bounds of
%               (-1,-1), (-1,1), (1,1), and (1,-1).
%
%INPUTS: None
%
%OUTPUTS: xi This is a 2XnumCubPoints set of points for the standard
%            square.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the standard square (4).
%
%This function implements the points given in [1] (8 points).
%
%EXAMPLE:
%We compare a 4th-order moment computed using these cubature points
%to one computed using monomialIntCube (a 5th order moment would have just
%been 0). The results are the same within typical finite precision limits.
% [xi,w]=fifthOrder2DCubPoints();
% alpha=[2;2];
% theMoment=findMomentFromSamp(alpha,xi,w);
% intVal=monomialIntCube(alpha);
% RelErr=(theMoment-intVal)/intVal
%
%REFERENCES:
%[1] F. D. Witherden and P. E. Vincent, "On the identification of symmetric
%    quadrature rules for finite element methods," Computer and Mathematics
%    with Applications, vol. 69, no. 10, pp. 1232-1241, May 2015.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

M=[0.68313005106397322554806924536807013272,                                          0,    0.8163265306122448979591836734693877551;
                                        0,   0.68313005106397322554806924536807013272,    0.8163265306122448979591836734693877551;
-0.68313005106397322554806924536807013272,                                          0,    0.8163265306122448979591836734693877551;
                                        0,  -0.68313005106397322554806924536807013272,    0.8163265306122448979591836734693877551;
  0.8819171036881968635005385845464201419,    0.8819171036881968635005385845464201419,    0.1836734693877551020408163265306122449;
  0.8819171036881968635005385845464201419,   -0.8819171036881968635005385845464201419,    0.1836734693877551020408163265306122449;
 -0.8819171036881968635005385845464201419,    0.8819171036881968635005385845464201419,    0.1836734693877551020408163265306122449;
 -0.8819171036881968635005385845464201419,   -0.8819171036881968635005385845464201419,    0.1836734693877551020408163265306122449];
w=M(:,3);
xi=M(:,1:2)';

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.


%% Data Structure: Boundary Conditions
%
% We use |bdFlag(1:NT,1:3)| to record the type of three edges of each
% triangle. Similarly in 3-D, we use |bdFlag(1:NT,1:4)| to record the type
% of four faces of each tetrahedron. The value is the type of boundary
% condition.
%
% * 0: non-boundary, i.e., an interior edge or face.
% * 1: first type, i.e., a Dirichlet boundary edge or face.
% * 2: second type, i.e., a Neumann boundary edge or face.
% * 3: third type, i.e., a Robin boundary edge or face.

%% Local labeling of edges and faces
% We label three edges of a triangle such that |bdFlag(t,i)| is the edge
% opposite to the i-th vertex. Similarly |bdFlag(t,i)| is the face opposite
% to the i-th vertex.

node = [1,0; 1,1; 0,0];
elem = [1 2 3];
edge = [2 3; 1 3; 1 2];
showmesh(node,elem);
findnode(node);
findedge(node,edge);

%% Set up boundary conditions
%
% The function |setboundary| is to set up the bdFlag matrix for a 2-D
% triangulation and |setboundary3| for a 3-D triangulation.
%
help setboundary

%%
% Note that if the i-th edge of t is on the boundary but |bdFlag(t,i)=0|,
% it is equivalent to use homogenous Neumann boundary condition (zero
% flux).

%% Example: Crack Domain
node = [1,0; 0,1; -1,0; 0,-1; 0,0; 1,0];        % nodes
elem = [5,1,2; 5,2,3; 5,3,4; 5,4,6];            % elements
elem = label(node,elem);                        % label the mesh
figure;
showmesh(node,elem);                            % plot mesh
findelem(node,elem);                            % plot element indices
findnode(node,2:6);                             % plot node indices
text(node(6,1),node(6,2)+0.075,int2str(1),'FontSize',16,'FontWeight','bold');
hold on;
plot([node(1,1),node(5,1)], [node(1,2),node(5,2)],'r-', 'LineWidth',3);
bdFlag = setboundary(node,elem,'Dirichlet');                % Dirichlet boundary condition
display(elem)
display(bdFlag)
%%
% node 1 and node 6 are the same point (1,0)

%%
bdFlag = setboundary(node,elem,'Dirichlet','abs(x) + abs(y) == 1','Neumann','y==0');
display(bdFlag)

%% Example: Prism Domain
node = [-1,-1,-1; 1,-1,-1; 1,1,-1; -1,1,-1; -1,-1,1; 1,-1,1; 1,1,1; -1,1,1];
elem = [1,2,3,7; 1,6,2,7; 1,5,6,7];
elem = label3(node,elem);
figure;
showmesh3(node,elem);
view([-53,8]);
findnode3(node,[1 2 3 5 6 7]);
findelem3(node,elem);
bdFlag = setboundary3(node,elem,'Dirichlet','(z==1) | (z==-1)');
display(elem)
display(bdFlag)

%%
% The top and bottom of the prism is set as Dirichlet boundary condition
% and other faces are zero flux boundary condition.

%% Remark
% It would save storage if we record boundary edges or faces only. The
% current data structure is convenient for the local refinement and
% coarsening since the boundary can be easily update along with the change
% of elements. The matrix |bdFlag| is sparse but we use a dense matrix to
% store it. We do not save |bdFlag| as a sparse matrix since updating
% sparse matrix is time consuming. We set up the type of |bdFlag| or
% |bdFlag| to |uint8| to minimize the waste of spaces.


function [ x, odata, opts ] = solver_TraceLS( A, b, lambda, x0, opts )
% SOLVER_TRACELS Unconstrained form of trace-regularized least-squares problem.
% [ x, odata, opts ] = solver_TraceLS( A, b, lambda, x0, opts )
%    Solves the trace-regularized least squares problem
%        minimize (1/2)*norm( A * X - b )^2 + lambda * trace( X )
%        with the constraint that X is positive semi-definite.
%    A must be a matrix or a linear operator, b must be a vector,
%    and lambda must be a real positive scalar.
%    The initial point x0 and option structure opts are
%    both optional.
%
%    If "A" is a sparse matrix, and nnz(Z) = length(b), then it is assumed
%    that the nonzero entries correspond to samples, and then the
%    corresponding sampling operator is used.
%
%    If opts.largescale = true, then uses an iterative method
%    to compute the eigenvalue decomposition used with prox_trace.
%
%   See also solver_L1RLS (aka The Lasso)

% Added Feb 7, 2011
error(nargchk(3,5,nargin));
if nargin < 4, x0 = []; end
if nargin < 5, opts = []; end
if ~isfield( opts, 'restart' ),
    opts.restart = 100;
end
if issparse(A) && nnz(A) == length(b)
    A = linop_subsample(A);
end
if isfield( opts,'largescale')
    prx = prox_trace( lambda, opts.largescale );
    opts = rmfield(opts,'largescale');
else
    prx = prox_trace( lambda );
end
% Note: the proximity operator of trace is really a combination
%  of the proximity operator of trace and the indicator
%  set of positive semi-definite matrices.
%  So we do not need to *explicitly* include the positive semi-definite constraint.

[x,odata,opts] = tfocs( smooth_quad, { A, -b }, prx, x0, opts );

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.