function n = navier_q2(xy,mv,flowsol)
%navier_q2  Q2 convection matrix
%   N = navier_q2(xy,ev,flowsol);
%   input
%          xy         Q2 nodal coordinate vector
%          ev         element mapping matrix
%          flowsol    Q2-Q1 or Q2-P1 flow solution
%   output
%          N          Q2 scalar convection matrix
%
%   Natural boundary conditions apply.
%   IFISS function: DJS; 11 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
nngpt=9;
x=xy(:,1); y=xy(:,2);
nvtx=length(x);   nel=length(mv(:,1));
usol=flowsol(1:nvtx); vsol=flowsol(nvtx+1:2*nvtx);
fprintf('setting up Q2 convection matrix...  ')
%
% initialise global matrices
      n = sparse(nvtx,nvtx);
%
%
% set up 3x3 Gauss points
      gpt=sqrt(0.6);
      s(1) = -gpt; t(1) = -gpt; wt(1)=25/81;
      s(2) =  gpt; t(2) = -gpt; wt(2)=25/81;
      s(3) =  gpt; t(3) =  gpt; wt(3)=25/81;
      s(4) = -gpt; t(4) =  gpt; wt(4)=25/81;
      s(5) =  0.0; t(5) = -gpt; wt(5)=40/81;
      s(6) =  gpt; t(6) =  0.0; wt(6)=40/81;
      s(7) =  0.0; t(7) =  gpt; wt(7)=40/81;
      s(8) = -gpt; t(8) =  0.0; wt(8)=40/81;
      s(9) =  0.0; t(9) =  0.0; wt(9)=64/81;
%
% inner loop over elements
        for ivtx = 1:4
        xl_v(:,ivtx) = x(mv(:,ivtx));
        yl_v(:,ivtx) = y(mv(:,ivtx));
        end
        for idx = 1:9
		xsl(:,idx) = usol(mv(:,idx));
		ysl(:,idx) = vsol(mv(:,idx));
		end
      ne = zeros(nel,9,9);
%
% loop over Gauss points
         for igpt = 1:nngpt
         sigpt=s(igpt);
         tigpt=t(igpt);
         wght=wt(igpt);
%  evaluate derivatives etc
         [jac,invjac,phi,dphidx,dphidy] = deriv(sigpt,tigpt,xl_v,yl_v);
         [psi,dpsidx,dpsidy] = qderiv(sigpt,tigpt,xl_v,yl_v);
         u_x = zeros(nel,1); u_y=zeros(nel,1);
            for k=1:9
		    u_x(:) = u_x(:) + xsl(:,k) .* psi(:,k);
		    u_y(:) = u_y(:) + ysl(:,k) .* psi(:,k);
		    end
		for j = 1:9
            for i = 1:9
				ne(:,i,j)  = ne(:,i,j)  + wght*u_x(:).*psi(:,i).*dpsidx(:,j);
                ne(:,i,j)  = ne(:,i,j)  + wght*u_y(:).*psi(:,i).*dpsidy(:,j);
             end
	    end
%
% end of Gauss point loop
         end
%
%%  element assembly into global matrix
      for krow=1:9
	  nrow=mv(:,krow);
          for kcol=1:9
		  ncol=mv(:,kcol);
          n = n + sparse(nrow,ncol,ne(:,krow,kcol),nvtx,nvtx);
	      end
       end
%
fprintf('done.\n')
return


% Chapter 4 - Electromagnetic Waves and Optical Resonators.
% Program_4b - Bifurcation Diagram for a Nonlinear Optical Resonator.
% Copyright Birkhauser 2013. Stephen Lynch.

% Bifurcation diagram for a simple fiber resonator (Figures 4.13 & 4.16(a)).
clear
halfN=1999;N=2*halfN+1;N1=1+halfN;
format long;E1=zeros(1,N);E2=zeros(1,N);
Esqr=zeros(1,N);Esqr1=zeros(1,N);ptsup=zeros(1,N);
C=0.345913;
E1(1)=0;kappa=0.0225;Pmax=120;phi=0;

% Ramp the power up
for n=1:halfN
    E2(n+1)=E1(n)*exp(1i*(abs(C*E1(n))^2-phi));
    E1(n+1)=1i*sqrt(1-kappa)*sqrt(n*Pmax/N1)+sqrt(kappa)*E2(n+1);
    Esqr(n+1)=abs(E1(n+1))^2;
end

% Ramp the power down
for n=N1:N
    E2(n+1)=E1(n)*exp(1i*(abs(C*E1(n))^2-phi));
    E1(n+1)=1i*sqrt(1-kappa)*sqrt(2*Pmax-n*Pmax/N1)+sqrt(kappa)*E2(n+1);
    Esqr(n+1)=abs(E1(n+1))^2;
end

for n=1:halfN
    Esqr1(n)=Esqr(N+1-n);
    ptsup(n)=n*Pmax/N1;
end

% Plot the bifurcation diagrams
fsize=15;
subplot(2,1,1)
plot(Esqr(1:N),'.','MarkerSize',1)
xlabel('Number of Ring Passes','FontSize',fsize);
ylabel('Output','FontSize',fsize);

subplot(2,1,2)
hold on
plot(ptsup(1:halfN),Esqr(1:halfN),'.','MarkerSize',1);
plot(ptsup(1:halfN),Esqr1(1:halfN),'.','MarkerSize',1);
xlabel('Input Power','FontSize',fsize);
ylabel('Output Power','FontSize',fsize);
hold off

% End of Program_4b.


%VEX Convert skew-symmetric matrix to vector
%
% V = VEX(S) is the vector (3x1) which has the skew-symmetric matrix S (3x3)
%
%           | 0   -vz  vy|
%           | vz   0  -vx|
%           |-vy   vx  0 |
%
% Notes::
% - This is the inverse of the function SKEW().
% - No checking is done to ensure that the matrix is actually skew-symmetric.
% - The function takes the mean of the two elements that correspond to each unique
%   element of the matrix, ie. vx = 0.5*(S(3,2)-S(2,3))
%
% See also SKEW.

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

function v = vex(S)
    if isrot(S) || ishomog(S)
        v = 0.5*[S(3,2)-S(2,3); S(1,3)-S(3,1); S(2,1)-S(1,2)];
    else
        error('argument must be a 3x3 matrix');
    end


function [ u, v, p ] = uvp_spiral ( nu, rho, n, x, y, t )

%*****************************************************************************80
%
%% UVP_SPIRAL returns velocity and pressure for the spiral flow.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Maxim Olshanskii, Leo Rebholz,
%    Application of barycenter refined meshes in linear elasticity
%    and incompressible fluid dynamics,
%    ETNA: Electronic Transactions in Numerical Analysis,
%    Volume 38, pages 258-274, 2011.
%
%  Parameters:
%
%    Input, real NU, the kinematic viscosity.
%
%    Input, real RHO, the fluid density.
%
%    Input, integer N, the number of nodes.
%
%    Input, real X(N), Y(N), the coordinates of nodes.
%
%    Input, real T, the current time.
%
%    Output, real U(N), V(N), the X and Y velocity.
%
%    Output, real P(N), the pressure.
%
  u = ( 1.0 + nu * t ) * 2.0 ...
    * x.^2 .* ( x - 1.0 ).^2 ...
    .* y .* ( 2.0 * y - 1.0 ) .* ( y - 1.0 );

  v = - ( 1.0 + nu * t ) * 2.0 ...
    * x .* ( 2.0 * x - 1.0 ) .* ( x - 1.0 ) ...
    .* y.^2 .* ( y - 1.0 ).^2;

  p = rho * y;

  return
end


function [U,T] = autoGen_cartPoleEnergy(x,q,dx,dq,m1,m2,g,l)
%AUTOGEN_CARTPOLEENERGY
%    [U,T] = AUTOGEN_CARTPOLEENERGY(X,Q,DX,DQ,M1,M2,G,L)

%    This function was generated by the Symbolic Math Toolbox version 6.3.
%    30-Nov-2015 20:13:07

t2 = cos(q);
U = -g.*l.*m2.*t2;
if nargout > 1
    t3 = dx+dq.*l.*t2;
    t4 = sin(q);
    T = dx.^2.*m1.*(1.0./2.0)+m2.*(t3.^2+dq.^2.*l.^2.*t4.^2).*(1.0./2.0);
end


function [VV,FF,SS] = sqrt3(V,F,varargin)
  % SQRT3 Perform sqrt-3 subdivision. After n iterations of loop subivision, the
  % resulting mesh will have
  %   3^n |F| faces
  %   (3^(n+1)-3)/2 |F| + |E| edges
  %   (3^(n+1)-3)/6 |F| + |V| vertices
  %
  % [VV,FF,SS] = sqrt3(V,F)
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by simplex-size list of simplex indices
  %   Optional:
  %     'Iterations' followed by number of recursive calls {1}
  % Outputs:
  %   VV  #VV by 3 list of output vertex positions
  %   FF  #FF by 3 list of triangle indices into VV
  %   SS  #VV by #V sparse matrix so that VV = SS*V

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

  SS = speye(size(V,1),size(V,1));
  VV = V;
  FF = F;
  for iter = 1:iters
    n = size(SS,1);
    m = size(FF,1);
    C = n+(1:m)';
    E = edges(FF);
    A = adjacency_matrix(FF);
    [AI,AJ] = find(A);
    val = sum(A,1);
    An = (4-2*cos(2*pi./val))./9;
    Andval = An./val;
    A = sparse(AI,AJ,Andval(AI),n,n) + (diag(sparse(1-An)));
    S = [A;sparse(repmat(1:m,3,1)',FF,1/3,m,n)];
    SS = S*SS;
    FF = [FF(:,2) FF(:,3) C;FF(:,3) FF(:,1) C;FF(:,1) FF(:,2) C];
    %BC = barycenter(V,F);
    %VV = [V;BC];
    FF = flip_edges(FF,E,'NonConflicting',true);
  end
  VV = SS*V;

  % F₁ ← 3F₀
  % E₁ ← E₀ + 3F₀
  % V₁ ← V₀ + F₀

end


function [ss,ssfun]=moesp(y,u,d)
% MOESP Multivariable Output Error State Space Approach of Subspace Identification
%   [SS,SSMAT] = MOESP(Y,U,D) identifies the observable subspace based on
%   the measured (N by ny) output matrix, Y with N  sampling points and ny
%   variables, and the corresponding N by nu input matrix, U. The third
%   parameter d is the embeded dimension, which should be larger than the
%   order of the system to be identified.
%
%   The function returns the scores of the subspace, SS and a function
%   handle SSMAT, for further identification of state space matrices.
%
%   The system order can determined from the returned score vector, SS so
%   that sum(SS(1:n)) ~ sum(SS).
%
%   Once the system order is determined, the underline dynamic system is
%   identified by calling the retruned function handle:
%
%   [A,B,C,D] = SSMAT(n)
%
%   to represent a state space model:
%
%       x(k+1) = Ax(k) + Bu(k)
%       y(k)   = Cx(k) + Du(k)
%
% See also: n4sid, subid

% Version 1.0 by Yi Cao at Cranfield University on 27th April 2008

% Reference
% Tohru Katayama, "Subspace Methods for System Identification", Springer
% 2005.

% Example
%{
%   Consider a multivariable fourth order system a,b,c,d
%   with two inputs and two outputs:
    a = [0.603 0.603 0 0;-0.603 0.603 0 0;0 0 -0.603 -0.603;0 0 0.603 -0.603];
    b = [1.1650,-0.6965;0.6268 1.6961;0.0751,0.0591;0.3516 1.7971];
    c = [0.2641,-1.4462,1.2460,0.5774;0.8717,-0.7012,-0.6390,-0.3600];
    d = [-0.1356,-1.2704;-1.3493,0.9846];
%
%   We take a white noise sequence of 1000 points as input u.
    N = 1000;
    u = randn(N,2);
%
%   With noise added, the state space system equations become:
%                  x_{k+1) = A x_k + B u_k + K e_k
%                    y_k   = C x_k + D u_k + e_k
%                 cov(e_k) = R
%
    k = [0.1242,-0.0895;-0.0828,-0.0128;0.0390,-0.0968;-0.0225,0.1459]*4;
    r = [0.0176,-0.0267;-0.0267,0.0497];
%
%   The noise input thus is equal to (the extra chol(r) makes cov(e) = r):
    e = randn(N,2)*chol(r);
%
%   And the simulated noisy output:
    y = dlsim(a,b,c,d,u) + dlsim(a,k,c,eye(2),e);
%
%   Using this output in subid returns a more realistic image of
%   the singular value plot:
    k = 10;
    [ss,ssfun] = moesp(y,u,k);
%
%   To determine the order, check the score vector
%
bar(ss)
%
%   Clearly, the order should be 4. Therefore, the state space model is
%   obtained by calling ssfun:
%
    [A,B,C,D]=ssfun(4);
%
% To compare with the actual system, we use the bode diagram:
    w = [0:0.005:0.5]*(2*pi); 		% Frequency vector
    m1 = dbode(a,b,c,d,1,1,w);
    m2 = dbode(a,b,c,d,1,2,w);
    M1 = dbode(A,B,C,D,1,1,w);
    M2 = dbode(A,B,C,D,1,2,w);
%
% Plot comparison
    figure(1)
    hold off;subplot;clg;
    subplot(221);plot(w/(2*pi),[m1(:,1),M1(:,1)]);title('Input 1 -> Output 1');
    subplot(222);plot(w/(2*pi),[m2(:,1),M2(:,1)]);title('Input 2 -> Output 1');
    subplot(223);plot(w/(2*pi),[m1(:,2),M1(:,2)]);title('Input 1 -> Output 2');
    subplot(224);plot(w/(2*pi),[m2(:,2),M2(:,2)]);title('Input 2 -> Output 2');
%}

% Input and output check
error(nargchk(1,3,nargin));
error(nargoutchk(0,4,nargout));

[ndat,ny]=size(y);
[mdat,nu]=size(u);
if ndat~=mdat
    error('Y and U have different length.')
end

% block Hankel matrix
N=ndat-d+1;
Y = zeros(d*ny,N);
U = zeros(d*nu,N);
sN=sqrt(N);
sy=y'/sN;
su=u'/sN;
for s=1:d
    Y((s-1)*ny+1:s*ny,:)=sy(:,s:s+N-1);
    U((s-1)*nu+1:s*nu,:)=su(:,s:s+N-1);
end

% LQ decomposition
R=triu(qr([U;Y]'))';
R=R(1:d*(ny+nu),:);

% SVD
R22 = R(d*nu+1:end,d*nu+1:end);
[U1,S1]=svd(R22);

% sigular value
ss = diag(S1);
% n=find(cumsum(ss)>0.85*sum(ss),1);

ssfun = @ssmat;

    function [A,B,C,D]=ssmat(n)
        % C and A
        Ok = U1(:,1:n)*diag(sqrt(ss(1:n)));
        C=Ok(1:ny,:);
        A=Ok(1:ny*(d-1),:)\Ok(ny+1:d*ny,:);

        % B and D
        L1 = U1(:,n+1:end)';
        R11 = R(1:d*nu,1:d*nu);
        R21 = R(d*nu+1:end,1:d*nu);
        M1 = L1*R21/R11;
        m = ny*d-n;
        M = zeros(m*d,nu);
        L = zeros(m*d,ny+n);
        for k=1:d
            M((k-1)*m+1:k*m,:)=M1(:,(k-1)*nu+1:k*nu);
            L((k-1)*m+1:k*m,:)=[L1(:,(k-1)*ny+1:k*ny) L1(:,k*ny+1:end)*Ok(1:end-k*ny,:)];
        end
        DB=L\M;
        D=DB(1:ny,:);
        B=DB(ny+1:end,:);
    end
end


function [ K, columns ] = spark( Phi )
%SPARK Calculates the spark of a given matrix Phi
% Let us get the rank of matrix
R = rank(Phi);
% Number of rows and columns of Phi
[~,N] = size(Phi);
for K=1:R+1
    % Possible number of choices of K columns
    % from N columns
    if K > N
        % This happens when we have
        % an NxN full rank matrix
        break;
    end
    numChoices = nchoosek(N,K);
    if (numChoices > 160000)
        error('N=%d, K=%d too large!',N,K);
    end
    % All possible choices of K columns out of N
    choices = nchoosek(1:N, K);
    % We iterate over choices
    for c=1:numChoices
        choice = choices(c, :);
        % We choose K columns out of N columns
        Phik = Phi (:, choice);
        % We compute the rank of this matrix
        r = rank(Phik);
        % We check if the columns are linearly
        % dependent.
        if r < K
            % We have found a set of columns
            % which are linearly dependent
            columns = choice;
            return
        end
    end
end
% All columns up to rank K are
% linearly independent
K = R+1;
columns = 1:N;
end


function res = UpSum(p)
%UPSUM        Rounded upwards result of sum(p)
%
%   resD = DownSum(p)
%
%On return, resD is sum(p) rounded upwards, also in the presence
%  of underflow. Input vector p may be single or double precision.
%
%Adapted from Algorithm 7.5 from
%  S.M. Rump, T. Ogita, S. Oishi: Accurate Floating-point Summation II:
%    Sign, K-fold Faithful and Rounding to Nearest, Siam J. Sci. Comput.,
%    31(2):1269-1302, 2008.
%Requires (4m+4k+4)n flops for m,k executions of the repeat-until loops
%  in the calls of TransformK, respectively.
%
%Reference implementation! Slow due to interpretation!
%

% written  03/03/07     S.M. Rump
% modified 05/09/09     S.M. Rump  rounding to nearest, complex input
%

  if ~isreal(p)
    res = complex(UpSum(real(p)),UpSum(imag(p)));
    return
  end

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

  if isa(p,'double')
    nmax = 2^26-2;                          % nmax = 67,108,864
  else
    nmax = 2^12-2;                          % nmax = 4,094
  end

  if length(p)<=nmax                        % not huge dimension
    [res,R,p,sigma,Ms] = TransformK(p,0);   % s-res = R+sum(p)
    delta = TransformK(p,R,sigma,Ms);       % delta faithful rounding of s-res
  else                                      % huge dimension
    [res,tau1,tau2,tau,p] = AccSumHugeN(p); % s=tau1+tau2+sum(tau)+sum(p)
    delta = AccSumHugeN([tau1;tau2;tau(:);p(:);-res]);
  end
  if delta>0                                % s > res
    res = succ(res);
  end

  if rndold
    setround(rndold)
  end


function [ssimval, ssimmap] = ssim(varargin)
%SSIM Structural Similarity Index for measuring image quality
%   SSIMVAL = SSIM(A, REF) calculates the Structural Similarity Index
%   (SSIM) value for image A, with the image REF as the reference. A and
%   REF can be 2D grayscale or 3D volume images, and must be of the same
%   size and class.
%
%   [SSIMVAL, SSIMMAP] = SSIM(A, REF) also returns the local SSIM value for
%   each pixel in SSIMMAP. SSIMMAP has the same size as A.
%
%   [SSIMVAL, SSIMMAP] = SSIM(A, REF, NAME1, VAL1,...) calculates the SSIM
%   value using name-value pairs to control aspects of the computation.
%   Parameter names can be abbreviated.
%
%   Parameters include:
%
%   'Radius'                 - Specifies the standard deviation of
%                              isotropic Gaussian function used for
%                              weighting the neighborhood pixels around a
%                              pixel for estimating local statistics. This
%                              weighting is used to avoid blocking
%                              artifacts in estimating local statistics.
%                              The default value is 1.5.
%
%   'DynamicRange'           - Positive scalar, L, that specifies the
%                              dynamic range of the input image. By
%                              default, L is chosen based on the class of
%                              the input image A, as L =
%                              diff(getrangefromclass(A)). Note that when
%                              class of A is single or double, L = 1 by
%                              default.
%
%   'RegularizationConstants'- Three-element vector, [C1 C2 C3], of
%                              non-negative real numbers that specifies the
%                              regularization constants for the luminance,
%                              contrast, and structural terms (see [1]),
%                              respectively. The regularization constants
%                              are used to avoid instability for image
%                              regions where the local mean or standard
%                              deviation is close to zero. Therefore, small
%                              non-zero values should be used for these
%                              constants. By default, C1 = (0.01*L).^2, C2
%                              = (0.03*L).^2, and C3 = C2/2, where L is the
%                              specified 'DynamicRange' value. If a value
%                              of 'DynamicRange' is not specified, the
%                              default value is used (see name-value pair
%                              'DynamicRange').
%
%   'Exponents'               - Three-element vector [alpha beta gamma],
%                               of non-negative real numbers that specifies
%                               the exponents for the luminance, contrast,
%                               and structural terms (see [1]),
%                               respectively. By default, all the three
%                               exponents are 1, i.e. the vector is [1 1
%                               1].
%
%   Notes
%   -----
%   1. A and REF can be arrays of upto three dimensions. All 3D arrays
%      are considered 3D volumetric images. RGB images will also be
%      processed as 3D volumetric images.
%
%   2. Input image A and reference image REF are converted to
%      floating-point type for internal computation.
%
%   3. For signed-integer images (int16), an offset is applied to bring the
%      gray values in the non-negative range before computing the SSIM
%      index.
%
%   Example
%   ---------
%   This example shows how to compute SSIM value for a blurred image given
%   the original reference image.
%
%   ref = imread('pout.tif');
%   H = fspecial('Gaussian',[11 11],1.5);
%   A = imfilter(ref,H,'replicate');
%
%   subplot(1,2,1); imshow(ref); title('Reference Image');
%   subplot(1,2,2); imshow(A);   title('Blurred Image');
%
%   [ssimval, ssimmap] = ssim(A,ref);
%
%   fprintf('The SSIM value is %0.4f.\n',ssimval);
%
%   figure, imshow(ssimmap,[]);
%   title(sprintf('SSIM Index Map - Mean SSIM Value is %0.4f',ssimval));
%
%   Class Support
%   -------------
%   Input arrays A and REF must be one of the following classes: uint8,
%   int16, uint16, single, or double. Both A and REF must be of the same
%   class. They must be nonsparse. SSIMVAL is a scalar and SSIMMAP is an
%   array of the same size as A. Both SSIMVAL and SSIMMAP are of class
%   double, unless A and REF are of class single in which case SSIMVAL and
%   SSIMMAP are of class single.
%
%   References:
%   -----------
%   [1] Z. Wang, A. C. Bovik, H. R. Sheikh, and E. P. Simoncelli, "Image
%       Quality Assessment: From Error Visibility to Structural
%       Similarity," IEEE Transactions on Image Processing, Volume 13,
%       Issue 4, pp. 600- 612, 2004.
%
%   See also IMMSE, MEAN, MEDIAN, PSNR, SUM, VAR.

%   Copyright 2013-2014 The MathWorks, Inc.

narginchk(2,10);

[A, ref, C, exponents, radius] = parse_inputs(varargin{:});

if isempty(A)
    ssimval = zeros(0, 'like', A);
    ssimmap = A;
    return;
end

if isa(A,'int16') % int16 is the only allowed signed-integer type for A and ref.
    % Add offset for signed-integer types to bring values in the
    % non-negative range.
    A = double(A) - double(intmin('int16'));
    ref = double(ref) - double(intmin('int16'));
elseif isinteger(A)
    A = double(A);
    ref = double(ref);
end

% Gaussian weighting function
gaussFilt = getGaussianWeightingFilter(radius,ndims(A));

% Weighted-mean and weighted-variance computations
mux2 = imfilter(A, gaussFilt,'conv','replicate');
muy2 = imfilter(ref, gaussFilt,'conv','replicate');
muxy = mux2.*muy2;
mux2 = mux2.^2;
muy2 = muy2.^2;

sigmax2 = imfilter(A.^2,gaussFilt,'conv','replicate') - mux2;
sigmay2 = imfilter(ref.^2,gaussFilt,'conv','replicate') - muy2;
sigmaxy = imfilter(A.*ref,gaussFilt,'conv','replicate') - muxy;

% Compute SSIM index
if (C(3) == C(2)/2) && isequal(exponents(:),ones(3,1))
    % Special case: Equation 13 from [1]
    num = (2*muxy + C(1)).*(2*sigmaxy + C(2));
    den = (mux2 + muy2 + C(1)).*(sigmax2 + sigmay2 + C(2));
    if (C(1) > 0) && (C(2) > 0)
        ssimmap = num./den;
    else
        % Need to guard against divide-by-zero if either C(1) or C(2) is 0.
        isDenNonZero = (den ~= 0);
        ssimmap = ones(size(A));
        ssimmap(isDenNonZero) = num(isDenNonZero)./den(isDenNonZero);
    end

else
    % General case: Equation 12 from [1]
    % Luminance term
    if (exponents(1) > 0)
        num = 2*muxy + C(1);
        den = mux2 + muy2 + C(1);
        ssimmap = guardedDivideAndExponent(num,den,C(1),exponents(1));
    else
        ssimmap = ones(size(A), 'like', A);
    end

    % Contrast term
    sigmaxsigmay = [];
    if (exponents(2) > 0)
        sigmaxsigmay = sqrt(sigmax2.*sigmay2);
        num = 2*sigmaxsigmay + C(2);
        den = sigmax2 + sigmay2 + C(2);
        ssimmap = ssimmap.*guardedDivideAndExponent(num,den,C(2),exponents(2));
    end

    % Structure term
    if (exponents(3) > 0)
        num = sigmaxy + C(3);
        if isempty(sigmaxsigmay)
            sigmaxsigmay = sqrt(sigmax2.*sigmay2);
        end
        den = sigmaxsigmay + C(3);
        ssimmap = ssimmap.*guardedDivideAndExponent(num,den,C(3),exponents(3));
    end

end

ssimval = mean(ssimmap(:));

end

% -------------------------------------------------------------------------
function component = guardedDivideAndExponent(num, den, C, exponent)

if C > 0
    component = num./den;
else
    component = ones(size(num),'like',num);
    isDenNonZero = (den ~= 0);
    component(isDenNonZero) = num(isDenNonZero)./den(isDenNonZero);
end

if (exponent ~= 1)
    component = component.^exponent;
end

end

function gaussFilt = getGaussianWeightingFilter(radius,N)
% Get 2D or 3D Gaussian weighting filter

filtRadius = ceil(radius*3); % 3 Standard deviations include >99% of the area.
filtSize = 2*filtRadius + 1;

if (N < 3)
    % 2D Gaussian mask can be used for filtering even one-dimensional
    % signals using imfilter.
    gaussFilt = fspecial('gaussian',[filtSize filtSize],radius);
else
    % 3D Gaussian mask
     [x,y,z] = ndgrid(-filtRadius:filtRadius,-filtRadius:filtRadius, ...
                    -filtRadius:filtRadius);
     arg = -(x.*x + y.*y + z.*z)/(2*radius*radius);
     gaussFilt = exp(arg);
     gaussFilt(gaussFilt<eps*max(gaussFilt(:))) = 0;
     sumFilt = sum(gaussFilt(:));
     if (sumFilt ~= 0)
         gaussFilt  = gaussFilt/sumFilt;
     end
end

end

function [A, ref, C, exponents, radius] = parse_inputs(varargin)

validImageTypes = {'uint8','uint16','int16','single','double'};

A = varargin{1};
validateattributes(A,validImageTypes,{'nonsparse','real'},mfilename,'A',1);

ref = varargin{2};
validateattributes(ref,validImageTypes,{'nonsparse','real'},mfilename,'REF',2);

if ~isa(A,class(ref))
    error(message('images:validate:differentClassMatrices','A','REF'));
end

if ~isequal(size(A),size(ref))
    error(message('images:validate:unequalSizeMatrices','A','REF'));
end

if (ndims(A) > 3)
    error(message('images:validate:tooManyDimensions','A and REF',3));
end

% Default values for parameters
dynmRange = diff(getrangefromclass(A));
C = [];
exponents = [1 1 1];
radius = 1.5;

args_names = {'dynamicrange', 'regularizationconstants','exponents',...
              'radius'};

for i = 3:2:nargin
    arg = varargin{i};
    if ischar(arg)
        idx = find(strncmpi(arg, args_names, numel(arg)));
        if isempty(idx)
            error(message('images:validate:unknownInputString', arg))

        elseif numel(idx) > 1
            error(message('images:validate:ambiguousInputString', arg))

        elseif numel(idx) == 1
            if (i+1 > nargin)
                error(message('images:validate:missingParameterValue'));
            end
            if idx == 1
                dynmRange = varargin{i+1};
                validateattributes(dynmRange,{'numeric'},{'positive', ...
                    'finite', 'real', 'nonempty','scalar'}, mfilename, ...
                    'DynamicRange',i);
                dynmRange = double(dynmRange);

            elseif idx == 2
                C = varargin{i+1};
                validateattributes(C,{'numeric'},{'nonnegative','finite', ...
                    'real','nonempty','vector', 'numel', 3}, mfilename, ...
                    'RegularizationConstants',i);
                C = double(C);

            elseif idx == 3
                exponents = varargin{i+1};
                validateattributes(exponents,{'numeric'},{'nonnegative', ...
                    'finite', 'real', 'nonempty','vector', 'numel', 3}, ...
                    mfilename,'Exponents',i);
                exponents = double(exponents);

            elseif idx == 4
                radius = varargin{i+1};
                validateattributes(radius,{'numeric'},{'positive','finite', ...
                    'real', 'nonempty','scalar'}, mfilename,'Radius',i);
                radius = double(radius);
            end
        end
    else
        error(message('images:validate:mustBeString'));
    end
end

% If 'RegularizationConstants' is not specified, choose default C.
if isempty(C)
    C = [(0.01*dynmRange).^2 (0.03*dynmRange).^2 ((0.03*dynmRange).^2)/2];
end

end


function filterSteerable( theta )
% Steerable 2D Gaussian derivative filter (for visualization).
%
% This function is a demonstration of steerable filters.  The directional
% derivative of G in an arbitrary direction theta can be found by taking a
% linear combination of the directional derivatives dxG and dyG.
%
% USAGE
%  filterSteerable( theta )
%
% INPUTS
%  theta   - orientation in radians
%
% OUTPUTS
%
% EXAMPLE
%  filterSteerable( pi/4 );
%
% See also filterGauss
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.0
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

% Get G
[x,y]=meshgrid(-1:.1:1, -1:.1:1 );
r = sqrt( x.^2 + y.^2 );
G = exp( -r .* r *2 );

% get first derivatives of G.  note: d/dx(G)=-2x*G
phi = atan2( y, x );
dxG = r .* cos(phi) .* G;
dyG = r .* sin(phi) .* G;

% get directional derivative by taking linear comb in theta
Gtheta = cos(theta)*dxG + sin(theta)*dyG;

% dislpay (scale for visualization purposes)
GS = cat(3,G,dxG*2,dyG*2,Gtheta*2);
figure(1); montage2( GS );


%LSPB  Linear segment with parabolic blend
%
% [S,SD,SDD] = LSPB(S0, SF, M) is a scalar trajectory (Mx1) that varies
% smoothly from S0 to SF in M steps using a constant velocity segment and
% parabolic blends (a trapezoidal velocity profile).  Velocity and
% acceleration can be optionally returned as SD (Mx1) and SDD (Mx1)
% respectively.
%
% [S,SD,SDD] = LSPB(S0, SF, M, V) as above but specifies the velocity of
% the linear segment which is normally computed automatically.
%
% [S,SD,SDD] = LSPB(S0, SF, T) as above but specifies the trajectory in
% terms of the length of the time vector T (Mx1).
%
% [S,SD,SDD] = LSPB(S0, SF, T, V) as above but specifies the velocity of
% the linear segment which is normally computed automatically and a time
% vector.
%
% LSPB(S0, SF, M, V) as above but plots S, SD and SDD versus time in a single
% figure.
%
% Notes::
% - If M is given
%   - Velocity is in units of distance per trajectory step, not per second.
%   - Acceleration is in units of distance per trajectory step squared, not
%     per second squared.
% - If T is given then results are scaled to units of time.
% - The time vector T is assumed to be monotonically increasing, and time
%   scaling is based on the first and last element.
% - For some values of V no solution is possible and an error is flagged.
%
% References::
% - Robotics, Vision & Control, Chap 3,
%   P. Corke, Springer 2011.
%
% See also TPOLY, JTRAJ.

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

%TODO
% add a 'dt' option, to convert to everything to units of seconds

function [s,sd,sdd] = lspb(q0, q1, t, V)

    t0 = t;
    if isscalar(t)
        t = (0:t-1)';
    else
        t = t(:);
    end
    plotsargs = {'Markersize', 16};

    tf = max(t(:));

    if nargin < 4
        % if velocity not specified, compute it
        V = (q1-q0)/tf * 1.5;
    else
        V = abs(V) * sign(q1-q0);
        if abs(V) < abs(q1-q0)/tf
            error('V too small');
        elseif abs(V) > 2*abs(q1-q0)/tf
            error('V too big');
        end
    end

    if q0 == q1
        s = ones(size(t)) * q0;
        sd = zeros(size(t));
        sdd = zeros(size(t));
        return
    end

    tb = (q0 - q1 + V*tf)/V;
    a = V/tb;

    p = zeros(length(t), 1);
    pd = p;
    pdd = p;

    for i = 1:length(t)
        tt = t(i);

        if tt <= tb
            % initial blend
            p(i) = q0 + a/2*tt^2;
            pd(i) = a*tt;
            pdd(i) = a;
        elseif tt <= (tf-tb)
            % linear motion
            p(i) = (q1+q0-V*tf)/2 + V*tt;
            pd(i) = V;
            pdd(i) = 0;
        else
            % final blend
            p(i) = q1 - a/2*tf^2 + a*tf*tt - a/2*tt^2;
            pd(i) = a*tf - a*tt;
            pdd(i) = -a;
        end
    end

    switch nargout
        case 0
            if isscalar(t0)
                % for scalar time steps, axis is labeled 1 .. M
                xt = t+1;
            else
                % for vector time steps, axis is labeled by vector M
                xt = t;
            end

            clf
            subplot(311)
            % highlight the accel, coast, decel phases with different
            % colored markers
            hold on
            %plot(xt, p);
            k = t<= tb;
            plot(xt(k), p(k), 'r.-', plotsargs{:});
            k = (t>=tb) & (t<= (tf-tb));
            plot(xt(k), p(k), 'b.-', plotsargs{:});
            k = t>= (tf-tb);
            plot(xt(k), p(k), 'g.-', plotsargs{:});
            grid; ylabel('$s$', 'FontSize', 16, 'Interpreter','latex');

            hold off

            subplot(312)
            plot(xt, pd, '.-', plotsargs{:});
            grid;
            if isscalar(t0)
                ylabel('$ds/dk$', 'FontSize', 16, 'Interpreter','latex');
            else
                ylabel('$ds/dt$', 'FontSize', 16, 'Interpreter','latex');
            end

            subplot(313)
            plot(xt, pdd, '.-', plotsargs{:});
            grid;
            if isscalar(t0)
                ylabel('$ds^2/dk^2$', 'FontSize', 16, 'Interpreter','latex');
            else
                ylabel('$ds^2/dt^2$', 'FontSize', 16, 'Interpreter','latex');
            end

            if ~isscalar(t0)
                xlabel('t (seconds)')
            else
                xlabel('k (step)');
                for c=findobj(gcf, 'Type', 'axes')
                    set(c, 'XLim', [1 t0]);
                end
            end
            shg
        case 1
            s = p;
        case 2
            s = p;
            sd = pd;
        case 3
            s = p;
            sd = pd;
            sdd = pdd;
    end


function d = ddiff ( d1, d2 )

%*****************************************************************************80
%
%% DDIFF returns the signed distance to a region that is the difference of two regions.
%
%  Discussion:
%
%    The author comments that this is not the true signed distance function for
%    the difference set, in particular, around corners.
%
%  Copyright:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real D1, D2, the signed distances to region 1 and 2.
%
%    Output, real D, the signed distance to the region formed by
%    removing from region 1 its intersection with region 2.
%
  d = max ( d1, -d2 );

  return
end


function [U, s, V] = fft_svd(PSF, center)
%
%   [U, s, V] = fft_svd(PSF, center);
%
%  Given a PSF this function computes an "SVD" factorization
%  for periodic boundary conditions.  That is, the "SVD" is
%  "Spectral Value Decomposition":
%            A = USV',
%  where U = V = inverse DFT matrix.
%
%  On Entry:
%     A  -  psfMatrix obect, with periodic boundary conditions
%
%  On Exit:
%     U, V  -  transformMatrix objects, with
%              U.transform = V.transform = 'fft'
%        s  -  column vector containing the eigenvalues
%              of A.  Note that these are not sorted from largest
%              smallest.
%
P = circshift(PSF, -(center - 1));
S = fftn(P);
s = S(:);

U = transformMatrix('fft');
U = U';
V = transformMatrix('fft');
V = V';


function table = r8mat_latinize ( m, n, table )

%*****************************************************************************80
%
%% R8MAT_LATINIZE "Latinizes" an R8MAT.
%
%  Discussion:
%
%    It is assumed, though not necessary, that the input dataset
%    has points that lie in the unit hypercube.
%
%    In any case, the output dataset will have this property.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of cells.
%
%    Input, real TABLE(M,N), the dataset to be "Latinized".
%
%    Output, real TABLE(M,N), the Latinized dataset.
%
  for i = 1 : m
    indx = r8vec_sort_heap_index_a ( n, table(i,1:n) );
    for j = 1 : n
      table(i,indx(j)) = ( 2 * j - 1 ) / ( 2 * n );
    end
  end

  return
end


function M = oned_bilinear ( kernel, phi, test, w_g )

%*****************************************************************************80
%
%% ONE_BILINEAR evaluates a bilinear form.
%
%  Discussion:
%
%    This function computes integral ( kernel * phi * test }
%
%    The calculation that is carried out is equivalent to:
%
%      [ n_gauss, n_test ] = size ( test );
%      [ n_gauss, n_dof ] = size ( phi );
%
%      M = zeros ( n_test, n_dof );
%      for i = 1 : n_test
%        for j = 1 : n_dof
%          M(i,j) = ( kernel' .* test(:,i)' ) * ( phi(:,j) .* w_g );
%        end
%      end
%
%  Parameters:
%
%    Input, real KERNEL(N_GAUSS), the kernel function in the integral,
%    evaluated at the Gauss points.
%
%    Input, real PHI(N_GAUSS,N_DOF), the element test functions evaluated
%    at the Gauss points.
%
%    Input, real TEST(N_GAUSS,N_TEST), the test functions evaluated at the
%    Gauss points.
%
%    Input, real W_G(N_GAUSS,1), a column vector of Gauss weights.
%
%    Output, real M(N_TEST,N_DOF), the bilinear form.
%
  M = test' * diag ( kernel .* w_g ) * phi;

  return
end


% The signless Laplacian matrix of a graph
% Def: the sum of the diagonal degree matrix and the adjacency matrix
%
% INPUTS: adjacency matrix, nxn
% OUTPUTs: signless Laplacian matrix, nxn
%
% GB: last updated, Dec 6 2015

function L=signlessLaplacian(adj)

L=diag(sum(adj))+adj;


function DataSet = prtDataGenMary
% prtDataGenMary  Generate unimodal M-ary example data
%
%  DATASET = prtDataGenMary returns a prtDataSetClass with randomly
%  generated data according to the following distribution.
%
%       H1: N([0 0],0.5*eye(2))
%       H2: N([0.5 0.5],0.1*eye(2))
%       H3: N([-2 -2],eye(2))
%
%  Example:
%
%  ds = prtDataGenMary;
%  plot(ds)
%
%   See also: prtDataSetClass, prtDataGenBiModal, prtDataGenIris,
%   prtDataGenMary, prtDataGenNoisySinc, prtDataGenOldFaithful,
%   prtDataGenSpiral, prtDataGenUnimodal, prtDataGenUnimodal, prtDataGenXor

rvH1 = prtRvMvn('mu',[0 0],'sigma',0.5*eye(2));
rvH2 = prtRvMvn('mu',[0.5 0.5],'sigma',0.1*eye(2));
rvH3 = prtRvMvn('mu',[-2 -2],'sigma',eye(2));
X = cat(1,draw(rvH1,100),draw(rvH2,100),draw(rvH3,100));
Y = prtUtilY(0,100,100,100);

DataSet = prtDataSetClass(X,Y,'name','prtDataGenMary');


%  Figure 3.4     Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%script to generate Fig. 3.4
%% fig3_04.m
%Satellite Pulse response Example. 3.16
clf;
numG=[0 0 0.0002];
denG=[1 0 0];
sysG=tf(numG,denG);
t=0:0.01:10;
%pulse input
u1=[zeros(1,500) 25*ones(1,10) zeros(1,491)];
[y1]=lsim(sysG,u1,t);
plot(t,u1);
grid;
xlabel('Time (sec)');
ylabel('Thrust Fc');
title('Fig. 3.4(a): Thrust input')
axis([0 10 0 26]);
pause;
% conversion to degrees
ff=180/pi;
y1=ff*y1;
plot(t,y1);
grid;
xlabel('Time (sec)');
ylabel('\theta (deg)');
title('Fig. 3.4(b): Satellite attitude')


function [x, infos] = ns_nmf(V, rank, in_options)
% Nonsmooth nonnegative matrix factorization (Nonsmooth-NMF)
%
% The problem of interest is defined as
%
%      min || V - W*S*H ||_F^2,
%
%      or
%
%      min  D(V||W*S*H),
%
%      where
%      {V, W, S, H} > 0.
%
% Given a non-negative matrix V, factorized non-negative matrices {W, S, H} are calculated.
%
%
% Inputs:
%       V           : (m x n) non-negative matrix to factorize
%       rank        : rank
%       in_options
%
%
% Output:
%       x           : non-negative matrix solution, i.e., x.W: (m x rank), x.H: (rank x n)
%       infos       : log information
%           epoch   : iteration nuber
%           cost    : objective function value
%           optgap  : optimality gap
%           time    : elapsed time
%           grad_calc_count : number of sampled data elements (gradient calculations)
%
% Reference:
%       A. Pascual-Montano, J. M. Carazo, K. Kochi, D. Lehmann, and R. D. Pascual-Marqui,
%       "Nonsmooth nonnegative matrix factorization (Nonsmooth-NMF),"
%       IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI), vol.28, no.3, pp.403-415, 2006.
%
%       Z. Yang, Y. Zhang, W. Yan, Y. Xiang, and S. Xie,
%       "A fast non-smooth nonnegative matrix factorization for learning sparse representation,"
%       IEEE Access, vol.4, pp.5161-5168, 2016.
%
%
% This file is part of NMFLibrary.
%
% This file is originally created by Graham Grindlay.
%
% 2010-01-14 Graham Grindlay (grindlay@ee.columbia.edu)
%
% Copyright (C) 2008-2010 Graham Grindlay (grindlay@ee.columbia.edu)
%
% 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/>.
%
%
% This file is partially created by Silja Polvi-Huttunen, University of Helsinki, Finland, 2014
%
%
% Created by modifiying the original code by H.Kasai on Jul. 23, 2018
%
% Change log:
%
%       Jul. 29, 2019 (Hiroyuki Kasai): Modified.
%
%       May. 20, 2019 (Hiroyuki Kasai): Added initialization module.
%
%       Jun. 24, 2022 (Hiroyuki Kasai): Fixed a bug of W normalization.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options.theta         = 0.5; % decides the degree in [0,1] of nonsmoothing (use 0 for standard NMF)
    local_options.metric_type   = 'euc'; % 'euc' (default) or 'kl-div'
    local_options.update_alg    = 'mu';  % 'mu' or 'apg'
    local_options.apg_maxiter   = 100;
    local_options.myeps         = 1e-16;
    local_options.norm_w        = 1;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W;
    H = init_factors.H;

    % initialize
    method_name = 'Nonsmooth-NMF';
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % initialize for this algorithm
    I = eye(rank);
    S = (1-options.theta) * I + (options.theta/rank) * ones(rank);

    % store initial info
    clear infos;
    WS = W * S;
    [infos, f_val, optgap] = store_nmf_info(V, WS, H, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        if strcmp(options.update_alg, 'mu')

            % update H
            WS = W * S;
            if strcmp(options.metric_type, 'euc')
                H = H .* (WS' * V) ./ ((WS' * WS) * H + 1e-9);
            elseif strcmp(options.metric_type, 'kl-div')
                H = H .* (WS' * (V./(WS * H + 1e-9))) ./ (sum(WS, 1)' * ones(1,n));
            else
                error('Invalid metric type')
            end

            % normalize rows in H
            [W, H] = normalize_WH(V, W, H, rank, 'type2');

            % update W
            SH = S * H;
            if strcmp(options.metric_type, 'euc')
                W = W .* (V * SH') ./ (W * (SH * SH') + 1e-9);
            elseif strcmp(options.metric_type, 'kl-div')
                W = W .* ((V ./ (W * SH + 1e-9)) * SH') ./ (ones(m, 1) * sum(SH,2)');
            end

        else % support only 'euc' metric.

            if 0
                % update H
                WS = W*S;
                [H, ~, ~] = nesterov_mnls_general(V, WS, [], H, 1, options.apg_maxiter, 'basic');

                % normalize rows in H
                [W, H] = normalize_WH(V, W, H, rank, 'type2');

                % update W
                SH = S*H;
                [W, ~, ~] = nesterov_mnls_general(V, [], SH', W, 1, options.apg_maxiter, 'basic');

            else

                % update W
                SH = S * H;
                [W, ~, ~] = nesterov_mnls_general(V, [], SH', W, 1, options.apg_maxiter, 'basic');
                %W_prev = W;
                W = W + (W<options.myeps) .* options.myeps;

                % normalize W
                if options.norm_w
                    %W11 = bsxfun(@rdivide,W,sqrt(sum(W.^2,1)));
                    W = normalize_W(W, 2);
                end

                % update H
                WS = W*S;
                [H, ~, ~] = nesterov_mnls_general(V, WS, [], H, 1, options.apg_maxiter, 'basic');

            end

        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % update epoch
        epoch = epoch + 1;

        % store info
        WS = W * S;
        infos = store_nmf_info(V, WS, H, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = W;
    x.H = H;
    x.S = S;

end


function h = plotConfEllipse(thetaOpt,confStats)
%PLOTCONFELLIPSE  Plot Confidence Ellipse/Ellipsoid

scov = confStats.Cov;
nparam = length(confStats.ConfInt);
ndata = size(confStats.ConfBnds.bnds,1);
clim = confStats.Conf;
ci = confStats.ConfInt;

%Find confidence ellipse/ellipsoid
[Ve,D] = eig(scov); e = diag(D); dfe = ndata-nparam; %note update ndata
scale = nparam*(ndata-1)/dfe * rmathlib('qf',clim,nparam,dfe);

if(length(thetaOpt)==2)
    t = linspace(0,2*pi,5e2)';
    uxc = sqrt(e(1)*scale)*cos(t); uyc = sqrt(e(2)*scale)*sin(t);
    c = ((Ve')\[uxc, uyc]')';
    xc = c(:,1) + thetaOpt(1); yc = c(:,2) + thetaOpt(2);
    modestr = 'Ellipse';
    h(1) = plot(thetaOpt(1),thetaOpt(2),'r*');
    hold on
    h(2) = plot(xc,yc,'b');
    h(3) = plot([-ci(1) -ci(1) ci(1) ci(1) -ci(1)]+thetaOpt(1),[-ci(2) ci(2) ci(2) -ci(2) -ci(2)]+thetaOpt(2),'k');
    hold off
    str = sprintf('%g%%',clim*100);
    legend('Least Squares Optimal Solution',[str ' Confidence ',modestr],['Rectangular ' str ' Confidence Interval'],'location','nw');
else
    optiwarn('opti:plotEllipse','3D Not implemented yet');
%     u = linspace(0,2*pi,100)'; v = linspace(0,pi,length(u))';
%     [U,V] = meshgrid(u,v);
%     uxc = sqrt(e(1)*scale).*cos(U).*sin(V); uyc = sqrt(e(2)*scale).*sin(U).*cos(V); uzc = sqrt(e(2)*scale)*cos(V);
%
%     c = ((Ve')\[uxc(:) uyc(:) uzc(:)]')';
%     xc = c(:,1) + thetaOpt(1); yc = c(:,2) + thetaOpt(2); zc = c(:,3) + thetaOpt(3);
%
%     n = length(u);
%     xc = reshape(xc,n,n);
%     yc = reshape(yc,n,n);
%     zc = reshape(zc,n,n);
%
%     h(2) = mesh(xc,yc,zc);
    modestr = 'Ellipsoid';
%     hold on
%         h(1) = plot3(thetaOpt(1),thetaOpt(2),thetaOpt(3),'r*');
%     hold off;
end
title(sprintf('Optimal Parameter %g%% Confidence %s',clim*100,modestr));
xlabel('\theta_1'); ylabel('\theta_2'); zlabel('\theta_3');


function loss=cost231(d,nf,kw)
%%
% d is the distance from the AP
% nf is the number of the floors encoutered
% kw is the vector cointaining the number of the type walls met
%  - the first element is for Light wall: plasterboard,particle board or thin
%    (<10 cm), light concrete wall.
%  - the second element is for Heavy wall: thick (>10cm), concrete or brick

% Loss for wall types in dB
Lw=0;
Lwa=[3.4 6.9];
for i=1:length(kw)
    Lw=Lw+kw(i)*Lwa(i);
end

% Loss for the floors in dB
Lfi=18.3;
Lf=20*log10(d)+nf^((nf+2)/(nf+1))*Lfi;

% Free space Path Loss in dB
lambda=0.125;
do=1;
Lo=20*log10((4*pi*do/lambda)^2);

loss=Lw+Lf+Lo;


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

%%
clear; close all; clc;

%% Syntax
% |[R_sq]=R_squared(y,yf);|

%% Description
% This function calculates the coefficient of determination (R-Squared) for
% the (e.g. measurement) data |y| and the data |yf| (e.g. a model fit).
% NaN entries in either inputs are ignored.

%% Examples
%

%%
% Plot settings
lineWidth=3;
markerSize=50;
fontSize=25;

%%
% Example data
t=linspace(0,2*pi,25);
y=sin(t)+0.1*randn(size(t));

%%
% Example fit
yf=sin(t);

%%
% Compute R-squared
[R_sq]=R_squared(y,yf)

%%
% Visualize

cFigure; hold on;
title(['R^2=',sprintf('%.4f',R_sq)],'Interpreter','Tex');
plot(t,y,'k.','MarkerSize',markerSize);
plot(t,yf,'g-','LineWidth',lineWidth);
set(gca,'FontSize',fontSize);
axis tight; axis square; box on; grid on;
drawnow;

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


function naca_test02 ( m, p, t )

%*****************************************************************************80
%
%% NACA_TEST02 tests NACA4_CAMBERED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real M, the maximum camber.
%    0 < M.
%
%    Input, real P, the location of maximum camber.
%    0.0 < P < 1.0.
%
%    Input, real T, the maximum relative thickness.
%    0.0 < T <= 1.0.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'NACA_TEST02\n' );
  fprintf ( 1, '  NACA4_CAMBERED evaluates (xu,yu) and (xl,yl) for a NACA\n' );
  fprintf ( 1, '  cambered airfoil defined by a 4-digit code.\n' );

  if ( nargin < 1 )
    m = 0.02;
  end

  if ( nargin < 2 )
    p = 0.4;
  end

  if ( nargin < 3 )
    t = 0.12;
  end

  c = 10.0;
  n = 51;

  xc = linspace ( 0.0, c, n );
  [ xu, yu, xl, yl ] = naca4_cambered ( m, p, t, c, xc );
%
%  Plot the wing surface.
%
  plot ( xu, yu, 'b-', ...
         xl(n:-1:1), yl(n:-1:1), 'b-', ...
         'Linewidth', 3 );

  axis equal
  grid on
  xlabel ( '<---X--->', 'Fontsize', 16 );
  ylabel ( '<---Y--->', 'Fontsize', 16 );
  title ( 'NACA 4-digit cambered airfoil', 'Fontsize', 24 );

  filename = 'cambered.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Graphics saved in file "%s"\n', filename );
%
%  Save it to a file.  List the points in counter clockwise order.
%
  xy = [ xl, xu(n:-1:1); yl, yu(n:-1:1) ];
  filename = 'cambered_data.txt';
  r8mat_write ( filename, 2, 2 * n, xy );
  fprintf ( 1, '  Data saved in file "%s"\n', filename );

  return
end


function rectangle_neighbor = bezier_surface_neighbors ( rectangle_num, ...
  rectangle_node )

%*****************************************************************************80
%
%% BEZIER_SURFACE_NEIGHBORS determines Bezier rectangle neighbors.
%
%  Discussion:
%
%    A (bicubic) Bezier surface is constructed of patches.  Each
%    patch is an (X,Y,Z) image of the unit rectangle in (U,V) space.
%    Two patches may be said to be "neighbors" if their images share
%    a side.
%
%    It is perfectly possible for each Bezier patch to have NO neighbors.
%    It is perfectly possible for each Bezier patch to have many neighbors
%    sharing a single side.
%
%    However, in the most common case, and the one handled here, we
%    may assume that the (X,Y,Z) patches fit together in a way similar
%    to the way quilt patches form a surface, so that most rectangles
%    have four neighbors, one on each side, while boundary rectangles
%    might have fewer neighbors.
%
%    This routine creates a data structure recording the neighbor information.
%
%    The primary amount of work occurs in sorting a list of
%    4 * RECTANGLE_NUM data items representing the sides of each patch.
%
%    The nodes of a single rectangle are assumed to be numbered as follows:
%
%    V
%    |   13-14-15-16
%    |    |  |  |  |
%    |    9-10-11-12
%    |    |  |  |  |
%    |    5--6--7--8
%    |    |  |  |  |
%    |    1--2--3--4
%    |
%    +--------------->U
%
%    We assume that a neighbor patch will agree in its (X,Y,Z) values
%    for all four nodes along a single side (although the nodes might
%    be listed in reverse order.)
%
%    We assume that there is at most one neighbor patch on each side.
%    (However, even in a simple case like the Utah teapot, this is not
%    true, because at the top and bottom, many patches have one side
%    that degenerates to a single point, and several such patches
%    meet at that point!)
%
%    We choose to number the sides of each patch as follows:
%
%
%    V      SIDE 3
%    |
%    |   13-14-15-16
%    | S  |  |  |  |
%    | I  9-10-11-12
%    | D  |  |  |  | SIDE 2
%    | E  5--6--7--8
%    |    |  |  |  |
%    | 4  1--2--3--4
%    |       SIDE 1
%    +--------------->U
%
%    And these indices for the sides correspond to the first index
%    in the RECTANGLE_NEIGHBOR array.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer RECTANGLE_NUM, the number of rectangles.
%
%    Input, integer RECTANGLE_NODE(16,RECTANGLE_NUM), the nodes that make up
%    each rectangle.
%
%    Output, integer RECTANGLE_NEIGHBOR(4,RECTANGLE_NUM), the indices of
%    the rectanges that are direct neighbors of a given rectangle.
%    RECTANGLE_NEIGHBOR(1,I) is the index of the rectangle which touches
%    side 1, defined by nodes 1 and 4, and so on.  RECTANGLE_NEIGHBOR(1,I)
%    is negative if there is no neighbor on that side.
%

%
%  Step 1.
%  From the list of vertices for rectangle T,
%  construct records that describe the four side segments, by listing
%  the first and last nodes of each segment.
%  To make matching easier later, we sort each pair of nodes.
%
  for rectangle = 1 : rectangle_num

    i1 = rectangle_node(1,rectangle);
    i2 = rectangle_node(4,rectangle);
    i3 = rectangle_node(13,rectangle);
    i4 = rectangle_node(16,rectangle);

    if ( i1 < i2 )
      row(4*(rectangle-1)+1,1:4) = [ i1, i2, 1, rectangle ];
    else
      row(4*(rectangle-1)+1,1:4) = [ i2, i1, 1, rectangle ];
    end

    if ( i2 < i4 )
      row(4*(rectangle-1)+2,1:4) = [ i2, i4, 2, rectangle ];
    else
      row(4*(rectangle-1)+2,1:4) = [ i4, i2, 2, rectangle ];
    end

    if ( i4 < i3 )
      row(4*(rectangle-1)+3,1:4) = [ i4, i3, 3, rectangle ];
    else
      row(4*(rectangle-1)+3,1:4) = [ i3, i4, 3, rectangle ];
    end

    if ( i3 < i1 )
      row(4*(rectangle-1)+4,1:4) = [ i3, i1, 4, rectangle ];
    else
      row(4*(rectangle-1)+4,1:4) = [ i1, i3, 4, rectangle ];
    end

  end
%
%  Step 2. Perform an ascending dictionary sort on the neighbor relations.
%  We only intend to sort on columns 1 and 2; the routine we call here
%  sorts on columns 1 through 3 but that won't hurt us.
%
%  What we need is to find cases where two rectangles share an edge.
%  Say they share an edge defined by the nodes I and J.  Then there are
%  two rows of ROW that start out ( I, J, ?, ? ).  By sorting ROW,
%  we make sure that these two rows occur consecutively.  That will
%  make it easy to notice that the rectangles are neighbors.
%
  row = i4row_sort_a ( 4*rectangle_num, 4, row );
%
%  Step 3. Neighboring rectangles show up as consecutive rows with
%  identical first two entries.  Whenever you spot this happening,
%  make the appropriate entries in RECTANGLE_NEIGHBOR.
%
  rectangle_neighbor(1:4,1:rectangle_num) = -1;

  irow = 1;

  while ( 1 )

    if ( 4 * rectangle_num <= irow )
      break
    end

    if ( row(irow,1) ~= row(irow+1,1) | row(irow,2) ~= row(irow+1,2) )
      irow = irow + 1;
      continue
    end

    side1 = row(irow,3);
    rectangle1 = row(irow,4);
    side2 = row(irow+1,3);
    rectangle2 = row(irow+1,4);

    rectangle_neighbor(side1,rectangle1) = rectangle2;
    rectangle_neighbor(side2,rectangle2) = rectangle1;

    irow = irow + 2;

  end

  return
end


function a = milnes ( m, n, x )

%*****************************************************************************80
%
%% MILNES returns the MILNES matrix.
%
%  Formula:
%
%    If ( I <= J )
%      A(I,J) = 1
%    else
%      A(I,J) = X(J)
%
%  Example:
%
%    M = 5, N = 5, X = ( 4, 7, 3, 8 )
%
%    1 1 1 1 1
%    4 1 1 1 1
%    4 7 1 1 1
%    4 7 3 1 1
%    4 7 3 8 1
%
%    M = 3, N = 6, X = ( 5, 7 )
%
%    1 1 1 1 1
%    5 1 1 1 1
%    5 7 1 1 1
%
%    M = 5, N = 3, X = ( 5, 7, 8 )
%
%    1 1 1
%    5 1 1
%    5 7 1
%    5 7 8
%    5 7 8
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    det ( A ) = ( 1 - X(1) ) * ( 1 - X(2) ) * ... * ( 1 - X(N-1) ).
%
%    A is singular if and only if X(I) = 1 for any I.
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Gregory, David Karney,
%    Example 3.14, Example 5.24,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, 1969, page 52, page 105,
%    LC: QA263.G68.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of A.
%
%    Input, real X(*), the lower column values.
%    If M <= N, then X should be dimensioned M-1.
%    If N < M, X should be dimensioned N.
%
%    Output, real A(M,N), the matrix.
%
  for i = 1 : m
    for j = 1 : n
      if ( i <= j )
        a(i,j) = 1.0;
      else
        a(i,j) = x(j);
      end
    end
  end

  return
end


%
% LibQPEP: A Library for Globally Optimal Solving Quadratic Pose Estimation Problems (QPEPs),
%          It also gives highly accurate uncertainty description of the solutions.
%
%
% Article:
%      Wu, J., Zheng, Y., Gao, Z., Jiang, Y., Hu, X., Zhu, Y., Jiao, J., Liu, M. (2020)
%           Quadratic Pose Estimation Problems: Globally Optimal Solutions,
%           Solvability/Observability Analysis and Uncertainty Description.
%           IEEE Transactions on Robotics.
%           https://doi.org/10.1109/TRO.2022.3155880
%
%
% Authors:      Jin Wu and Ming Liu
% Affiliation:  Hong Kong University of Science and Technology (HKUST)
% Emails:       jin_wu_uestc@hotmail.com; eelium@ust.hk
% Websites:     https://zarathustr.github.io
%               https://ram-lab.com
%
%
% syms_hand_eye.m: Generating symbolic expressions for solving hand-eye
%                  calibration problem

clear all
close all
clc

addpath('func_files');
addpath('utils');

syms A11 A12 A13 A14 real
syms A21 A22 A23 A24 real
syms A31 A32 A33 A34 real

A = [
    A11, A12, A13, A14;
    A21, A22, A23, A24;
    A31, A32, A33, A34;
      0,   0,   0,   1;
      ];

syms B11 B12 B13 B14 real
syms B21 B22 B23 B24 real
syms B31 B32 B33 B34 real

B = [
    B11, B12, B13, B14;
    B21, B22, B23, B24;
    B31, B32, B33, B34;
      0,   0,   0,   1;
      ];

syms q0 q1 q2 q3 real
syms t1 t2 t3 lambda real
q = [q0; q1; q2; q3];
tt = [t1; t2; t3];
RR = q2R(q);
XX = [
    RR, tt;
    zeros(1, 3), 1];
J_pure = J_func_hand_eye(XX, A, B);
[coef_J_pure, mon_J_pure] = coeffs(J_pure, [q; tt]);
generateFuncFile(coef_J_pure, fullfile('func_files', 'coef_J_pure_hand_eye_func_new.m'), {A(1 : 3, :), B(1 : 3, :)});
generateFuncFile(mon_J_pure, fullfile('func_files', 'mon_J_pure_hand_eye_func_new.m'), {q, tt});
J = J_pure - 0.5 * lambda * (q.' * q - 1);
x = [q; tt; lambda];
Jacob = jacobian(J, x);
Jacob = expand(Jacob.');

coef_len = length(coef_J_pure);
str = sprintf('coef_J = sym(''coef_J'', [1, %d]);', coef_len);
eval(str);
showSyms(symvar(coef_J));
J = coef_J * mon_J_pure.' - 0.5 * lambda * (q.' * q - 1);
x = [q; tt; lambda];
Jacob = jacobian(J, x);
Jacob = expand(Jacob.');

[coeft1, mont1] = coeffs(Jacob(5), tt);
g1 = coeft1(1 : 3).';
[coeftq1, montq1] = coeffs(expand(Jacob(5) - g1.' * tt ), q);
generateFuncFile(coeftq1, fullfile('func_files', 'coeftq1_hand_eye_func_new.m'), {coef_J});
[coeft2, mont2] = coeffs(Jacob(6), tt);
g2 = coeft2(1 : 3).';
[coeftq2, montq2] = coeffs(expand(Jacob(6) - g2.' * tt ), q);
generateFuncFile(coeftq2, fullfile('func_files', 'coeftq2_hand_eye_func_new.m'), {coef_J});
[coeft3, mont3] = coeffs(Jacob(7), tt);
g3 = coeft3(1 : 3).';
[coeftq3, montq3] = coeffs(expand(Jacob(7) - g3.' * tt ), q);
generateFuncFile(coeftq3, fullfile('func_files', 'coeftq3_hand_eye_func_new.m'), {coef_J});
G = - [g1.'; g2.'; g3.'];
generateFuncFile(G, fullfile('func_files', 'G_hand_eye_func_new.m'), {coef_J});

pinvG = sym('pinvG', [3, 3]);
showSyms(symvar(pinvG));
coefs_tq = sym('coefs_tq', [3, 11]);
showSyms(symvar(coefs_tq));
ts = pinvG * coefs_tq * montq1.';
t1 = ts(1);
t2 = ts(2);
t3 = ts(3);
generateFuncFile(t1, fullfile('func_files', 't1_hand_eye_func_new.m'), {pinvG, coefs_tq, q});
generateFuncFile(t2, fullfile('func_files', 't2_hand_eye_func_new.m'), {pinvG, coefs_tq, q});
generateFuncFile(t3, fullfile('func_files', 't3_hand_eye_func_new.m'), {pinvG, coefs_tq, q});

[coef_Jacob1_qt, mon_Jacob1_qt] = coeffs(Jacob(1) + lambda * q0, [q; tt]);
generateFuncFile(coef_Jacob1_qt, fullfile('func_files', 'coef_Jacob1_qt_hand_eye_func_new.m'), {coef_J});
[coef_Jacob2_qt, mon_Jacob2_qt] = coeffs(Jacob(2) + lambda * q1, [q; tt]);
generateFuncFile(coef_Jacob2_qt, fullfile('func_files', 'coef_Jacob2_qt_hand_eye_func_new.m'), {coef_J});
[coef_Jacob3_qt, mon_Jacob3_qt] = coeffs(Jacob(3) + lambda * q2, [q; tt]);
generateFuncFile(coef_Jacob3_qt, fullfile('func_files', 'coef_Jacob3_qt_hand_eye_func_new.m'), {coef_J});
[coef_Jacob4_qt, mon_Jacob4_qt] = coeffs(Jacob(4) + lambda * q3, [q; tt]);
generateFuncFile(coef_Jacob4_qt, fullfile('func_files', 'coef_Jacob4_qt_hand_eye_func_new.m'), {coef_J});

coef_Jacob1_qt_syms = sym('coef_Jacob1_qt_syms', [1, 36]);
coef_Jacob2_qt_syms = sym('coef_Jacob2_qt_syms', [1, 36]);
coef_Jacob3_qt_syms = sym('coef_Jacob3_qt_syms', [1, 36]);
coef_Jacob4_qt_syms = sym('coef_Jacob4_qt_syms', [1, 36]);
showSyms(symvar(coef_Jacob1_qt_syms));
showSyms(symvar(coef_Jacob2_qt_syms));
showSyms(symvar(coef_Jacob3_qt_syms));
showSyms(symvar(coef_Jacob4_qt_syms));
coef_Jacob_qt_syms = [
    coef_Jacob1_qt_syms;
    coef_Jacob2_qt_syms;
    coef_Jacob3_qt_syms;
    coef_Jacob4_qt_syms;
    ];
Jacob_ = coef_Jacob_qt_syms * mon_Jacob1_qt.';

eqs = expand(eval([Jacob_; Jacob(8)]));

f0 = eqs(1);
f1 = eqs(2);
f2 = eqs(3);
f3 = eqs(4);
[coef_f0_q, mon_f0_q] = coeffs(f0, q);
[coef_f1_q, mon_f1_q] = coeffs(f1, q);
[coef_f2_q, mon_f2_q] = coeffs(f2, q);
[coef_f3_q, mon_f3_q] = coeffs(f3, q);
coef_f0_q_sym = sym('coef_f0_q_sym', [1, 24]);
coef_f1_q_sym = sym('coef_f1_q_sym', [1, 24]);
coef_f2_q_sym = sym('coef_f2_q_sym', [1, 24]);
coef_f3_q_sym = sym('coef_f3_q_sym', [1, 24]);
showSyms(symvar(coef_f0_q_sym));
showSyms(symvar(coef_f1_q_sym));
showSyms(symvar(coef_f2_q_sym));
showSyms(symvar(coef_f3_q_sym));

coef_f_q_sym = [
    coef_f0_q;
    coef_f1_q;
    coef_f2_q;
    coef_f3_q;
    ];
generateFuncFile(coef_f_q_sym, fullfile('func_files', 'coef_f_q_sym_hand_eye_func_new.m'), {pinvG, coefs_tq, coef_Jacob_qt_syms});

f0 = coef_f0_q_sym * mon_f0_q.';
f1 = coef_f1_q_sym * mon_f1_q.';
f2 = coef_f2_q_sym * mon_f2_q.';
f3 = coef_f3_q_sym * mon_f3_q.';
eq = expand([
     f0 * q1 - f1 * q0;
     f0 * q2 - f2 * q0;
     f0 * q3 - f3 * q0;
     q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 - 1;
     ]);
generateFuncFile(eq, fullfile('func_files', 'eq_hand_eye_func_new.m'), {coef_f0_q_sym, coef_f1_q_sym, coef_f2_q_sym, coef_f3_q_sym, q});

eq_ = eq;
eq_ = expand(eval(subs(eq_, q0^2, (1 - q1^2 - q2^2 - q3^2))));
[coefeq1, moneq1] = coeffs(eq_(1), q);
mon2th1 = [ moneq1(10), moneq1(14), moneq1(16), moneq1(23), moneq1(27), moneq1(29), moneq1(33), moneq1(35), moneq1(37)];
mon4th1 = [ moneq1(1 : 9), moneq1(11 : 13), moneq1(15), moneq1(17 : 22), moneq1(24 : 26), moneq1(28), moneq1(30 : 32), moneq1(34), moneq1(36), moneq1(38)];

[coefeq2, moneq2] = coeffs(eq_(2), q);
mon2th2 = [ moneq2(10), moneq2(14), moneq2(16), moneq2(23), moneq2(27), moneq2(29), moneq2(33), moneq2(35), moneq2(37)];
mon4th2 = [ moneq2(1 : 9), moneq2(11 : 13), moneq2(15), moneq2(17 : 22), moneq2(24 : 26), moneq2(28), moneq2(30 : 32), moneq2(34), moneq2(36), moneq2(38)];

[coefeq3, moneq3] = coeffs(eq_(3), q);
mon2th3 = [ moneq3(10), moneq3(14), moneq3(16), moneq3(23), moneq3(27), moneq3(29), moneq3(33), moneq3(35), moneq3(37)];
mon4th3 = [ moneq3(1 : 9), moneq3(11 : 13), moneq3(15), moneq3(17 : 22), moneq3(24 : 26), moneq3(28), moneq3(30 : 32), moneq3(34), moneq3(36), moneq3(38)];

coef2th1 = [ coefeq1(10), coefeq1(14), coefeq1(16), coefeq1(23), coefeq1(27), coefeq1(29), coefeq1(33), coefeq1(35), coefeq1(37)];
coef4th1 = [ coefeq1(1 : 9), coefeq1(11 : 13), coefeq1(15), coefeq1(17 : 22), coefeq1(24 : 26), coefeq1(28), coefeq1(30 : 32), coefeq1(34), coefeq1(36), coefeq1(38)];

coef2th2 = [ coefeq2(10), coefeq2(14), coefeq2(16), coefeq2(23), coefeq2(27), coefeq2(29), coefeq2(33), coefeq2(35), coefeq2(37)];
coef4th2 = [ coefeq2(1 : 9), coefeq2(11 : 13), coefeq2(15), coefeq2(17 : 22), coefeq2(24 : 26), coefeq2(28), coefeq2(30 : 32), coefeq2(34), coefeq2(36), coefeq2(38)];

coef2th3 = [ coefeq3(10), coefeq3(14), coefeq3(16), coefeq3(23), coefeq3(27), coefeq3(29), coefeq3(33), coefeq3(35), coefeq3(37)];
coef4th3 = [ coefeq3(1 : 9), coefeq3(11 : 13), coefeq3(15), coefeq3(17 : 22), coefeq3(24 : 26), coefeq3(28), coefeq3(30 : 32), coefeq3(34), coefeq3(36), coefeq3(38)];

y = mon2th1.';
generateFuncFile(y, fullfile('func_files', 'y_func_hand_eye_new.m'), {q});
v = mon4th1(1 : end - 1).';
generateFuncFile(v, fullfile('func_files', 'v_func_hand_eye_new.m'), {q});

D = [
    coef4th1(1 : end - 1);
    coef4th2(1 : end - 1);
    coef4th3(1 : end - 1);
    ];

G = [
    coef2th1;
    coef2th2;
    coef2th3;
    ];

c = [
    coef4th1(end);
    coef4th2(end);
    coef4th3(end);
    ];

generateFuncFile(D, fullfile('func_files', 'D_func_hand_eye_new.m'), {coef_f0_q_sym, coef_f1_q_sym, coef_f2_q_sym, coef_f3_q_sym});
generateFuncFile(G, fullfile('func_files', 'G_func_hand_eye_new.m'), {coef_f0_q_sym, coef_f1_q_sym, coef_f2_q_sym, coef_f3_q_sym});
generateFuncFile(c, fullfile('func_files', 'c_func_hand_eye_new.m'), {coef_f0_q_sym, coef_f1_q_sym, coef_f2_q_sym, coef_f3_q_sym});

HH = jacobian(eq, q);
generateFuncFile(HH, fullfile('func_files', 'Jacob_hand_eye_func_new.m'), {coef_f0_q_sym, coef_f1_q_sym, coef_f2_q_sym, coef_f3_q_sym, q});
gradient = expand(HH.' * eq);

[coef1, mon1] = coeffs(eqs(1), x);
[coef2, mon2] = coeffs(eqs(2), x);
[coef3, mon3] = coeffs(eqs(3), x);
[coef4, mon4] = coeffs(eqs(4), x);

Q_sym = [
    coef1(11), coef1(18), coef1(22), coef1(24);
    coef2(11), coef2(18), coef2(22), coef2(24);
    coef3(11), coef3(18), coef3(22), coef3(24);
    coef4(11), coef4(18), coef4(22), coef4(24);
    ];
generateFuncFile(Q_sym, fullfile('func_files', 'Q_hand_eye_func_new.m'), {pinvG, coefs_tq, coef_Jacob_qt_syms});

res = expand(eqs(1 : 4) - Q_sym * q);
H = jacobian(res, q);
h1 = H(:, 1);
h2 = H(:, 2);
h3 = H(:, 3);
h4 = H(:, 4);
P1 = jacobian(h1, q);
P2 = jacobian(h2, q);
P3 = jacobian(h3, q);
P4 = jacobian(h4, q);

for i = 1 : 4
    for j = 1 : 4
        str = sprintf('W%d%d = [jacobian(P%d(1, :), q%d).''; jacobian(P%d(2, :), q%d).''; jacobian(P%d(3, :), q%d).''; jacobian(P%d(4, :), q%d).''];', ...
                        i, j, i, j - 1, i, j - 1, i, j - 1, i, j - 1);
        eval(str);
    end
end

W1 = [W11, W12, W13, W14];
W2 = [W21, W22, W23, W24];
W3 = [W31, W32, W33, W34];
W4 = [W41, W42, W43, W44];
v = kron(q, q);
u = kron(v, q);
W = - [W1, W2, W3, W4] / 6;
generateFuncFile(W, fullfile('func_files', 'W_hand_eye_func_new.m'), {pinvG, coefs_tq, coef_Jacob_qt_syms});


function yred=rednoise(N,g,a);
% REDNOISE: A fast rednoise generator using filter.
%
% example: rednoise(N,g);
%
% Description: generates a rednoise series
% with zero process mean.
% note: statistical mean&variance will be different.
%
% Inputs: n - desired length of time series
%         g - lag-1 autocorrelation
%         a - noise innovation variance parameter (optional, default=1)
%
% Example:
%   plot(rednoise(1000,.95))
%
% Aslak Grinsted 2006-2014

% -------------------------------------------------------------------------
%The MIT License (MIT)
%
%Copyright (c) 2014 Aslak Grinsted
%
%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.
%---------------------------------------------------------------------------

if nargin<3
    a=1;
end

if g==0
    yred=randn(N,1)*a;
    return
end
tau=ceil(-2/log(abs(g))); %2 x de-correlation time

yred=filter([1 0],[1;-g],randn(tau+N,1)*a);
yred=yred(tau+1:end);


%   getBox:  Given the current state, returns a number from 1 to 162
%             designating the region of the state space encompassing the current state.
%             Returns a value of -1 if a failure state is encountered.

function box = getBox(theta,thetaDot,x,xDot)
theta = rad2deg(theta);
thetaDot = rad2deg(thetaDot);
if (x < -2.4 || x > 2.4  || theta < -12 || theta > 12)
    box = -1;
else

if (theta<-6&&theta>=-12)
	thetaBucket = 1;
elseif (theta<-1&&theta>=-6)
	thetaBucket = 2;
elseif (theta<0&&theta>=-1)
	thetaBucket = 3;
elseif (theta<1&&theta>=0)	% zero included
	thetaBucket = 4;
elseif (theta<6&&theta>=1)
	thetaBucket = 5;
elseif (theta<=12&&theta>=6)
	thetaBucket = 6;
end

if (x<-0.8&&x>=-2.4)
	xBucket = 1;
elseif (x<=0.8&&x>=-0.8)
	xBucket = 2;
elseif (x<=2.4&&x>0.8)
	xBucket = 3;
end

if (xDot<-0.5)
	xDotBucket = 1;
elseif (xDot>=-0.5&&xDot<=0.5)
	xDotBucket = 2;
else
	xDotBucket = 3;
end

if (thetaDot<-50)
	thetaDotBucket = 1;
elseif (thetaDot>=-50&&thetaDot<=50)
	thetaDotBucket = 2;
else
	thetaDotBucket = 3;
end

box = sub2ind([6,3,3,3],thetaBucket, thetaDotBucket,xBucket,xDotBucket);
end
return;


% Calculate border score for a firing map
%
% Calculates a border score for a firing rate map according to the article "Representation of Geometric
% Borders in the Entorhinal Cortex" by Solstad et. al. (Science 2008).
% Border score ranges from -1 to +1 with +1 being "a perfect border cell". If the firing map contains
% no firing fields, then the returned score is -1.
% The score reflects not only how close a field is to a border and how big the coverage of this field is,
% but also it reflects spreadness of a field. The best border score (+1) will be calculated for a thin
% line (1 px, bin) that lies along the wall and fully covers this wall.
%
%  USAGE
%   score = analyses.borderScore(map, fieldsMap, fields, <options>)
%   map         A 2D firing map, not a struct you get from analyses.map, but actual values.
%   fieldsMap   Matrix of the same size as map with only non-zero elements
%               belonging to detected fields. This is an output from
%               function analyses.placefield.
%   fields      Array of structures with information about detected fields.
%               This is an output from function analyses.placefield.
%   <options>   Optional list of property-value pairs (see table below)
%
%   ==============================================================================================
%    Properties    Values
%   ----------------------------------------------------------------------------------------------
%    'searchWidth'  If map is not perfect, but contains NaN values along borders, then
%                   search for border pixels can have NaNs. To mitigate this, we check
%                   searchWidth rows/columns near border and if the closest to the border pixel
%                   equals to NaN, we search for first non-NaN value in searchWidth rows-columns.
%                   This argument is optional and default value is 8.
%    'walls'        Definition of walls along which the border score is calculated. Provided by
%                   a string which contains characters that stand for walls:
%                   T - top wall (we assume the bird-eye view on the arena)
%                   R - right wall
%                   B - bottom wall
%                   L - left wall
%                   Characters are case insensitive. Default value is 'TRBL' meaning that border
%                   score is calculated along all walls. Any combination is possible, e.g.
%                   'R' to calculate along right wall, 'BL' to calculate along two walls, e.t.c.
%   ==============================================================================================
%   score       Border score. Ranges from -1 to +1 (if there are no fields,
%               then score is -1).
%
%  SEE
%   See analyses.placefield
%
function score = borderScore(map, fieldsMap, fields, varargin)
    score = -1;

    if ~isempty(fields)
        coverage = analyses.borderCoverage(fields, varargin{:});

        fieldsFiringMap = fieldsMap;
        fieldsFiringMap(fieldsFiringMap > 0) = 1; % different fields are assigned different numbers,
                                                  % make them all 1.
        fieldsFiringMap = map .* fieldsFiringMap;

        fdist = weighted_firing_distance(fieldsFiringMap);
        score = (coverage - fdist)/(coverage + fdist);
    end
end

function wfd = weighted_firing_distance(map)
    % normalized firing map. Normalization is done by
    % the sum of firing rates of all pixels belonging to all fields
    map = map/nansum(map(:));

    [ly, lx] = size(map);
    [mx, my] = meshgrid(1:lx, 1:ly);

    % alternative code for distance matrix is:
    % map = zeros(ly, lx);
    % map(1, :) = 1;
    % map(end, :) = 1;
    % map(:, 1) = 1;
    % map(:, end) = 1;
    % D = bwdist(map);
    % but this code is slower.

    distance_matrix = min(min(my,mx), min(flipud(my), fliplr(mx)));

    wfd = nansum(nansum(map .* distance_matrix));

    % normalization by half of the smallest arena size min(ly, lx)/2.
    wfd = (2 * wfd) / min(ly, lx);
end


%% [core,U] = tensor_hosvd(T,t,r)
%
%%% Normal HoSVD
% [core,U] = tensor_hosvd(T)
% [core,U] = tensor_hosvd(T,0)
% [core,U] = tensor_hosvd(T,[0 0 0])
%
%%% Truncate mode-3 basis matrice (keep the first 2 eigenvalues)
% [core,U] = tensor_hosvd(T,[0 0 2])
%
%%% Perform mode-3 rank-2 partial svd
% [core,U] = tensor_hosvd(T,0,[0 0 2])
%
function [core,U] = tensor_hosvd(T,t,r)
  core = T;

  if(nargin < 3) r = zeros(1,ndims(T)); end
  if(nargin < 2) t = zeros(1,ndims(T)); end

  if(t == 0) t = zeros(1,ndims(T)); end
  if(r == 0) r = zeros(1,ndims(T)); end

  for i = 1:ndims(T)
    M{i} = double(tenmat(T,i));
    %M{i} = unfolding(double(T),i);

    %%% perform partial svd
    if(r(i) > 0)
      [U{i},S{i},V{i}] = svds(M{i},r(i),'L');
    else
      [U{i},S{i},V{i}] = svd(M{i});
    end

    %%% truncate basis matrice
    if(t(i) > 0)
      U{i}(:,t(i)+1:end) = 0;
    end

    %[m n] = size(U{i});
    %s(i) = n;
    %core = tensor(folding(U{i}'*unfolding(double(core),i), i, s));
    %core = double(ttm(tensor(core),U{i},i,'t'));
    core = ttm(core,U{i}',i);
  end
end


function [ n_data, x, fx ] = struve_h1_values ( n_data )

%*****************************************************************************80
%
%% STRUVE_H1_VALUES returns some values of the Struve H1 function.
%
%  Discussion:
%
%    The function is defined by:
%
%      H1(x) = 2*x/pi * Integral ( 0 <= t <= pi/2 )
%        sin ( x * cos ( t ) )^2 * sin ( t ) dt
%
%    In Mathematica, the function can be evaluated by:
%
%      StruveH[1,x]
%
%    The data was reported by McLeod.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Allan McLeod,
%    Algorithm 757, MISCFUN: A software package to compute uncommon
%      special functions,
%    ACM Transactions on Mathematical Software,
%    Volume 22, Number 3, September 1996, pages 288-301.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 20;

  fx_vec = [ ...
     0.80950369576367526071E-06, ...
     0.12952009724113229165E-04, ...
     0.82871615165407083021E-03, ...
     0.13207748375849572564E-01, ...
     0.19845733620194439894E+00, ...
     0.29853823231804706294E+00, ...
     0.64676372828356211712E+00, ...
     0.10697266613089193593E+01, ...
     0.38831308000420560970E+00, ...
     0.74854243745107710333E+00, ...
     0.84664854642567359993E+00, ...
     0.58385732464244384564E+00, ...
     0.80600584524215772824E+00, ...
     0.53880362132692947616E+00, ...
     0.72175037834698998506E+00, ...
     0.58007844794544189900E+00, ...
     0.60151910385440804463E+00, ...
     0.70611511147286827018E+00, ...
     0.61631110327201338454E+00, ...
     0.62778480765443656489E+00 ];

  x_vec = [ ...
        0.0019531250E+00, ...
       -0.0078125000E+00, ...
        0.0625000000E+00, ...
       -0.2500000000E+00, ...
        1.0000000000E+00, ...
        1.2500000000E+00, ...
        2.0000000000E+00, ...
       -4.0000000000E+00, ...
        7.5000000000E+00, ...
       11.0000000000E+00, ...
       11.5000000000E+00, ...
      -16.0000000000E+00, ...
       20.0000000000E+00, ...
       25.0000000000E+00, ...
      -30.0000000000E+00, ...
       50.0000000000E+00, ...
       75.0000000000E+00, ...
      -80.0000000000E+00, ...
      100.0000000000E+00, ...
     -125.0000000000E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function inside = polygon_contains_point_2d ( n, v, p )

%*****************************************************************************80
%
%% POLYGON_CONTAINS_POINT_2D finds if a point is inside a simple polygon in 2D.
%
%  Discussion:
%
%    A simple polygon is one whose boundary never crosses itself.
%    The polygon does not need to be convex.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    M Shimrat,
%    Position of Point Relative to Polygon,
%    ACM Algorithm 112,
%    Communications of the ACM,
%    Volume 5, Number 8, page 434, August 1962.
%
%  Parameters:
%
%    Input, integer N, the number of nodes or vertices in the polygon.
%    N must be at least 3.
%
%    Input, real V(2,N), the coordinates of the vertices of the polygon.
%
%    Input, real P(2), the coordinates of the point to be tested.
%
%    Output, logical INSIDE, is TRUE if the point is inside the polygon.
%
  inside = 1;

  for i = 1 : n

    x1 = v(1,i);
    y1 = v(2,i);

    if ( i < n )
      x2 = v(1,i+1);
      y2 = v(2,i+1);
    else
      x2 = v(1,1);
      y2 = v(2,1);
    end

    if ( ( y1 < p(2) & p(2) <= y2 ) | ( p(2) <= y1 & y2 < p(2) ) )
      if ( ( p(1) - x1 ) - ( p(2) - y1 ) * ( x2 - x1 ) / ( y2 - y1 ) < 0.0 )
        inside = ~inside;
      end
    end

  end

  inside = ~inside;

  return
end


function compspec
% Competing species phase diagram
%    using MATLAB ode
%
%   $Ekkehard Holzbecher  $Date: 2006/09/04 $
%--------------------------------------------------------------------------
T = 1000;                % maximum time
r = [1; 1];              % rates
e = [1; 1];              % equilibria
lambda = 1;              % lambda parameter
c0 = [1; 1];             % initial concentrations

gtraj = 1;               % trajectory plot
gquiv = 20;              % arrow field plot; value for no. of arrows in 1D
xmin = 0; xmax = 1;      % x- interval for arrow field plot
ymin = 0; ymax = 1;      % y-     "     "    "     "     "
scale = 2;               % scaling factor for arrows
%----------------------execution-------------------------------------------

options = odeset('AbsTol',1e-20);
[~,c] = ode15s(@CS,[0 T],c0,options,r,e,lambda);

%---------------------- graphical output ----------------------------------

if (gtraj)
    plot (c(:,1)',c(:,2)'); hold on;
    plot (e(1),0,'s'); plot (0,e(2),'s');
    legend ('trajectory');
    xlabel ('specie 1'); ylabel ('specie 2');
    title ('Competing species');
end
if (gquiv)
    [x,y] = meshgrid (linspace(xmin,xmax,gquiv),linspace(ymin,ymax,gquiv));
    dy = zeros(gquiv,gquiv,2);
    for i = 1:gquiv
        for j = 1:gquiv
            dy(i,j,:) = CS(0,[x(i,j);y(i,j)],r,e,lambda);
        end
    end
    quiver (x,y,dy(:,:,1),dy(:,:,2),scale);
end

%---------------------- function ------------------------------------------
function dydt = CS(~,y,r,e,lambda)
k = [e(1)/(1+lambda*y(2)/y(1)); e(2)/(1+y(1)/y(2)/lambda)];
dydt = r.*y.*(1-y./k);


function [ r, seed ] = r8_uniform_01 ( seed )

%*****************************************************************************80
%
%% R8_UNIFORM_01 returns a unit pseudorandom R8.
%
%  Discussion:
%
%    This routine implements the recursion
%
%      seed = 16807 * seed mod ( 2^31 - 1 )
%      r8_uniform_01 = seed / ( 2^31 - 1 )
%
%    The integer arithmetic never requires more than 32 bits,
%    including a sign bit.
%
%    If the initial seed is 12345, then the first three computations are
%
%      Input     Output      R8_UNIFORM_01
%      SEED      SEED
%
%         12345   207482415  0.096616
%     207482415  1790989824  0.833995
%    1790989824  2035175616  0.947702
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley Interscience, page 95, 1998.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer SEED, the integer "seed" used to generate
%    the output random number.  SEED should not be 0.
%
%    Output, real R, a random value between 0 and 1.
%
%    Output, integer SEED, the updated seed.  This would
%    normally be used as the input seed on the next call.
%
  i4_huge = 2147483647;

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8_UNIFORM_01 - Fatal error!' );
  end

  seed = floor ( seed );

  seed = mod ( seed, i4_huge );

  if ( seed < 0 )
    seed = seed + i4_huge;
  end

  k = floor ( seed / 127773 );

  seed = 16807 * ( seed - k * 127773 ) - k * 2836;

  if ( seed < 0 )
    seed = seed + i4_huge;
  end

  r = seed * 4.656612875E-10;

  return
end


% PDELDEMO This program creates the left preconditionied PDE example
%          from Chapter 3
%
% The example has the form
%
% L(u) = f, 0 < x,y < 1
% with homogeneous Dirichlet BC
%
% In terms of the program below, the equation is
% Lu = - (u_xx + u_yy) + 20*u*(u_x + u_y)  = f
%
% Within the nonlinear map, pdefun.m, we apply fish2d.m as
% a preconditioner.
%
% The action of the laplacian, partial wrt x, and partial wrt y
% are computed in the routines lapmf, dxmf, and dymf. Here mf
% stands for MATRIX FREE.
%
global rhsf prhsf;
m=31; n=m*m;
h=1/(m+1);
h2=(m+1)*(m+1);
h1=(m+1);
tolh=1/h2;
mres=3;
%
% set up the equation
%
x=1:m;
e=x';
x=x'*h;
%
% Form the solution: sol(x,y)= 10 x y (1-x) (1-y) exp(x^4.5)
%
zsoly=x.*(1-x);
zsolx=zsoly.*exp(x.^4.5);
solt=10*zsolx*zsoly';
sol=solt(:);
clear rhs; clear axt; clear at; clear ayt; clear solt;

% fix the right side so that the solution is known
%
% unpreconditioned and right preconditioned
%
rhsf = lapmf(sol) + 20*sol.*(dxmf(sol)+dymf(sol));
%
% left preconditioned
%
prhsf = sol + 20*fish2d(sol.*(dxmf(sol)+dymf(sol)));
%
% Set the initial iterate to zero. Call the solver three times with
% each of the Krylov methods
%
u0=zeros(m*m,1);
%
tol=[1.d-1,1.d-1]*tolh;
%
% GMRES
%
x=zeros(n,1); parms = [40,40,.9,1];
[sol, it_histg, ierr] = nsoli(x,'pdeleft',tol,parms);
%
% BICGSTAB
%
x=zeros(n,1); parms = [40,40,.9,3];
[sol, it_histb, ierr] = nsoli(x,'pdeleft',tol,parms);
%
% TFQMR
%
x=zeros(n,1); parms = [40,40,.9,4];
[sol, it_histt, ierr] = nsoli(x,'pdeleft',tol,parms);
figure(1)
ng=length(it_histg(:,1));
nb=length(it_histb(:,1));
nt=length(it_histt(:,1));
semilogy(0:ng-1,it_histg(:,1)/it_histg(1,1),'-',...
0:nb-1,it_histb(:,1)/it_histg(1,1),'--',...
0:nt-1,it_histt(:,1)/it_histg(1,1),'-.');
xlabel('Nonlinear iterations');
ylabel('Relative residual');
legend('GMRES','BICGSTAB','TFQMR');
figure(2)
semilogy(it_histg(:,2),it_histg(:,1)/it_histg(1,1),'-',...
it_histb(:,2),it_histb(:,1)/it_histb(1,1),'--',...
it_histt(:,2),it_histt(:,1)/it_histt(1,1),'-.');
xlabel('Function evaluations');
ylabel('Relative residual');
legend('GMRES','BICGSTAB','TFQMR');


function SmoothY=fastsmooth(Y,w,type,ends)
% fastsmooth(Y,w,type,ends) smooths vector Y with smooth
%  of width w. Version 3.0, October 2016.
% The argument "type" determines the smooth type:
%   If type=1, rectangular (sliding-average or boxcar)
%   If type=2, triangular (2 passes of sliding-average)
%   If type=3, pseudo-Gaussian (3 passes of sliding-average)
%   If type=4, pseudo-Gaussian (4 passes of same sliding-average)
%   If type=5, multiple-width (4 passes of different sliding-average)
% The argument "ends" controls how the "ends" of the signal
% (the first w/2 points and the last w/2 points) are handled.
%   If ends=0, the ends are zero.  (In this mode the elapsed
%     time is independent of the smooth width). The fastest.
%   If ends=1, the ends are smoothed with progressively
%     smaller smooths the closer to the end. (In this mode the
%     elapsed time increases with increasing smooth widths).
% fastsmooth(Y,w,type) smooths with ends=0.
% fastsmooth(Y,w) smooths with type=1 and ends=0.
% Examples:
% fastsmooth([1 1 1 10 10 10 1 1 1 1],3)= [0 1 4 7 10 7 4 1 1 0]
%
% fastsmooth([1 1 1 10 10 10 1 1 1 1],3,1,1)= [1 1 4 7 10 7 4 1 1 1]
%
% x=1:100;
% y=randn(size(x));
% plot(x,y,x,fastsmooth(y,5,3,1),'r')
% xlabel('Blue: white noise.    Red: smoothed white noise.')
%
% Copyright (c) 2012, Thomas C. O'Haver
%
% 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.
if nargin==2, ends=0; type=1; end
if nargin==3, ends=0; end
  switch type
    case 1
       SmoothY=sa(Y,w,ends);
    case 2
       SmoothY=sa(sa(Y,w,ends),w,ends);
    case 3
       SmoothY=sa(sa(sa(Y,w,ends),w,ends),w,ends);
    case 4
       SmoothY=sa(sa(sa(sa(Y,w,ends),w,ends),w,ends),w,ends);
    case 5
       SmoothY=sa(sa(sa(sa(Y,round(1.6*w),ends),round(1.4*w),ends),round(1.2*w),ends),w,ends);
  end

function SmoothY=sa(Y,smoothwidth,ends)
w=round(smoothwidth);
SumPoints=sum(Y(1:w));
s=zeros(size(Y));
halfw=round(w/2);
L=length(Y);
for k=1:L-w,
   s(k+halfw-1)=SumPoints;
   SumPoints=SumPoints-Y(k);
   SumPoints=SumPoints+Y(k+w);
end
s(k+halfw)=sum(Y(L-w+1:L));
SmoothY=s./w;
% Taper the ends of the signal if ends=1.
  if ends==1,
  startpoint=(smoothwidth + 1)/2;
  SmoothY(1)=(Y(1)+Y(2))./2;
  for k=2:startpoint,
     SmoothY(k)=mean(Y(1:(2*k-1)));
     SmoothY(L-k+1)=mean(Y(L-2*k+2:L));
  end
  SmoothY(L)=(Y(L)+Y(L-1))./2;
  end