function [Neurons] = neural_gas(D,n,epochs,alpha0,lambda0)

%NEURAL_GAS Quantizes the data space using the neural gas algorithm.
%
% Neurons = neural_gas(D, n, epochs, [alpha0], [lambda0])
%
%   C = neural_gas(D,50,10);
%   sM = som_map_struct(sD);
%   sM.codebook = neural_gas(sD,size(sM.codebook,1),10);
%
%  Input and output arguments ([]'s are optional):
%   D          (matrix) the data matrix, size dlen x dim
%              (struct) a data struct
%   n          (scalar) the number of neurons
%   epochs     (scalar) the number of training epochs (the number of
%                       training steps is dlen*epochs)
%   [alpha0]   (scalar) initial step size, 0.5 by default
%   [lambda0]  (scalar) initial decay constant, n/2 by default
%
%   Neurons    (matrix) the neuron matrix, size n x dim
%
% See also SOM_MAKE, KMEANS.

% References:
%  T.M.Martinetz, S.G.Berkovich, and K.J.Schulten. "Neural-gas" network
%  for vector quantization and its application to time-series prediction.
%  IEEE Transactions on Neural Networks, 4(4):558-569, 1993.

% Contributed to SOM Toolbox vs2, February 2nd, 2000 by Juha Vesanto
% Copyright (c) by Juha Vesanto
% http://www.cis.hut.fi/projects/somtoolbox/

% juuso 101297 020200

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Check arguments and initialize

error(nargchk(3, 5, nargin));  % check the number of input arguments

if isstruct(D), D = D.data; end
[dlen,dim] = size(D);
Neurons = (rand(n,dim)-0.5)*10e-5; % small initial values
train_len = epochs*dlen;

if nargin<4 || isempty(alpha0) || isnan(alpha0), alpha0 = 0.5; end
if nargin<5 || isempty(lambda0) || isnan(lambda0), lambda0 = n/2; end

% random sample order
rand('state',sum(100*clock));
sample_inds = ceil(dlen*rand(train_len,1));

% lambda
lambda = lambda0 * (0.01/lambda0).^([0:(train_len-1)]/train_len);

% alpha
alpha = alpha0 * (0.005/alpha0).^([0:(train_len-1)]/train_len);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Action

for i=1:train_len,

  % sample vector
  x = D(sample_inds(i),:); % sample vector
  known = ~isnan(x);       % its known components
  X = x(ones(n,1),known);  % we'll need this

  % neighborhood ranking
  Dx = Neurons(:,known) - X;  % difference between vector and all map units
  [~, inds] = sort((Dx.^2)*known'); % 1-BMU, 2-BMU, etc.
  ranking(inds) = [0:(n-1)];
  h = exp(-ranking/lambda(i));
  H = h(ones(length(known),1),:)';

  % update
  Neurons = Neurons + alpha(i)*H.*(x(ones(n,1),known) - Neurons(:,known));

  % track
  fprintf(1,'%d / %d \r',i,train_len);
  if 0 && mod(i,50) == 0,
    hold off, plot3(D(:,1),D(:,2),D(:,3),'bo')
    hold on, plot3(Neurons(:,1),Neurons(:,2),Neurons(:,3),'r+')
    drawnow
  end
end

fprintf(1,'\n');

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


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


function out = comp_loss(E,loss)

switch loss
    case 'l1'
        out = norm(E(:),1);
    case 'l21'
        out = 0;
        for i = 1 : size(E,2)
            out = out + norm(E(:,i));
        end
    case 'l2'
        out = 0.5*norm(E,'fro')^2;
end


function f16=f16(x)

Bound=[-5 5];

if nargin==0
    f16 = Bound;
else
    f16=4*x(1,:)^2-2.1*x(1,:)^4+(1/3)*x(1,:)^6+x(1,:)*x(2,:)-4*x(2,:)^2+4*x(2,:)^4;
end


function [hpb] = kW2hpb(kW)
% Convert power from kilowatts to boiler horsepower.
% Chad A. Greene 2012
hpb = kW*0.101941995;


% 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 [tests,pass,perf]=test_woody(varargin)

inputs={'verbose'};
verbose=0;
for n=1:nargin
    if(~isempty(varargin{n}))
        eval([inputs{n} '=varargin{n};']);
    end
end

%Test the examples
% Note: This requires the Signal Processing toolbox
str=['t=[0:1/1000:1];N=1001;x=sin(2*pi*t)+sin(4*pi*t)+sin(8*pi*t);' ...
    'y=exp(0.01*[-1*[500:-1:1] 0 -1*[1:500]]);'...
  's=x.*y;sig1=0;sig2=0.1;M=100;S=zeros(N,M);center=501;TAU=round((rand(1,M)-0.5)*160);'...
  'for i=1:M;tau=TAU(i);if(tau<0)' ...
  'S(:,i)=[s(-1*tau:end)''; zeros(-1*(tau+1),1)];'...
  'else; S(:,i)=[zeros(tau,1);s(1:N-tau)''; ];end;'...
  'if(i<50) S(:,i)=S(:,i) + randn(N,1).*sig1; else; S(:,i)=S(:,i) + randn(N,1).*sig2; end; end; '...
  '[wood]=woody(S,[],[],''woody'',''biased'');[thor]=woody(S,[],[],''thornton'',''biased'');'...
  'figure;subplot(211);' ...
  'plot(s,''b'',''LineWidth'',2);hold on;plot(S,''r'');plot(s,''b'',''LineWidth'',2);legend(''Signal'',''Measurements'');'...
  'subplot(212);plot(s);hold on;plot(mean(S,2),''r'');plot(wood,''g'');plot(thor,''k'');'...
  'legend(''Signal'',''Normal Ave'',''Woody Ave'',''Thornton Ave'');close all;'];

test_string={str};

clean_up={};
[tests,pass,perf]=test_wrapper(test_string,clean_up,verbose);


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 obstacle_pts = random_obstacles(dim, n_obs, lb, ub, scale)
% Generate a randomly distributed field of random obstacles. Each
% obstacle consists of 2^dim points uniformly randomly distributed
% within a 2*scale-length cube around a randomly selected center point
if nargin < 5
  scale = 1;
end
offsets = scale * (rand(dim, 2^dim * n_obs) * 2 - 1);
centers = bsxfun(@plus, bsxfun(@times, rand(dim, n_obs), ub - lb), lb);
centers = reshape(centers, dim, 1, []);
obstacle_pts = bsxfun(@plus, centers, reshape(offsets ./ (n_obs^(1/dim)), [dim, 2^dim, n_obs]));

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


%TROT2 SE2 rotation matrix
%
% T = TROT2(THETA) is a homogeneous transformation (3x3) representing a rotation
% of THETA radians.
%
% T = TROT2(THETA, 'deg') as above but THETA is in degrees.
%
% Notes::
% - Translational component is zero.
%
% See also ROT2, TRANSL2, TROTX, TROTY, TROTZ.

% 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 T = trot2(t, varargin)
	T = [rot2(t, varargin{:}) [0 0]'; 0 0 1];


function OUT = StarVariable(DATA)
% Computes foreign variables as in the GVAR with equal weights
% =======================================================================
% [OUT, wgts] = xstar(DATA)
% -----------------------------------------------------------------------
% INPUTS
%   - DATA: an (T x N) matrix of time series
%--------------------------------------------------------------------------
% OUTPUT
%    - OUT: (T x N) matrix of xstar time series
% =======================================================================
% Ambrogio Cesa Bianchi, March 2015
% ambrogio.cesabianchi@gmail.com

[nobs, nvar] = size(DATA);
OUT = nan(nobs, nvar);
w = ones(1,nvar)./nvar;

for ii=1:nvar
    % Create a logical vector to exclude variable ii
    select = ones(1,nvar); select(ii)=0; select = logical(select);
    % Remove variable ii from DATA
    data = DATA(:,select);
    % Remove weight/flow ii from w
    weights = w(select);
    % Compute xstar variable for cuntry ii
    OUT(:,ii) = CrossWeightAverage(data,weights);
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 [Mean2, Var2] = online_UpdateWin(Mean, Var, old_x, new_x, n)
%Using this function to update mean and variance
%
%Input
%	Mean	: average of previous data
%	Var     : variance of previous data
%	old_x	: oldest data in sliding window
%	new_x	: new arriving data
%	n		: number of previous data
%
%Output
%	Mean2	: result of average by online updating average with new data
%	Var2	: result of variance by online updating average with new data
%
    dim = size(Var);
	if n <= 0
		Mean = x;
		Var = ones(dim)*0.5;
	else
		Mean2 = ( n * Mean - old_x + new_x ) / n;
		Var2 = max((( n * ( Var + Mean.^2 ) - old_x.^2 + new_x.^2 ) / n - Mean2.^2),0);
	end
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 );


% [INPUT]
% r = A float t-by-n matrix (-Inf,Inf) representing the logarithmic returns.
% sst = A float (0.0,0.1] representing the statistical significance threshold for the linear Granger-causality test (optional, default=0.05).
% rp = A boolean indicating whether to use robust p-values for the linear Granger-causality test (optional, default=false).
%
% [OUTPUT]
% am = A binary n-by-n matrix {0;1} representing the adjcency matrix.

function am = causal_adjacency(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('r',@(x)validateattributes(x,{'double'},{'real' '2d' 'nonempty'}));
        ip.addOptional('sst',0.05,@(x)validateattributes(x,{'double'},{'real' 'finite' '>' 0 '<=' 0.1 'scalar'}));
        ip.addOptional('rp',false,@(x)validateattributes(x,{'logical'},{'scalar'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    r = validate_input(ipr.r);
    sst = ipr.sst;
    rp = ipr.rp;

    nargoutchk(1,1);

    am = causal_adjacency_internal(r,sst,rp);

end

function am = causal_adjacency_internal(r,sst,rp)

    up = isempty(getCurrentTask());

    n = size(r,2);

    nan_indices = any(isnan(r),1);
    nok = sum(~nan_indices);

    seq = (1:n).';
    seq(nan_indices) = [];

    i = repelem(seq,nok,1);
    j = repmat(seq,nok,1);

    indices = i == j;
    i(indices) = [];
    j(indices) = [];

    r_in = arrayfun(@(x)r(:,x),i,'UniformOutput',false);
    r_out = arrayfun(@(x)r(:,x),j,'UniformOutput',false);

    k = nok^2 - nok;
    pvals = zeros(k,1);

    if (rp)
        if (up)
            parfor y = 1:k
                [~,pvals(y)] = linear_granger_causality(r_in{y},r_out{y});
            end
        else
            for y = 1:k
                [~,pvals(y)] = linear_granger_causality(r_in{y},r_out{y});
            end
        end
    else
        if (up)
            parfor y = 1:k
                [pvals(y),~] = linear_granger_causality(r_in{y},r_out{y});
            end
        else
            for y = 1:k
                [pvals(y),~] = linear_granger_causality(r_in{y},r_out{y});
            end
        end
    end

    am = zeros(n);
    am(sub2ind([n n],i,j)) = pvals < sst;

end

function [b,c,r] = hac_regression(y,x,ratio)

    t = length(y);

    [b,~,r] = regress(y,x);

    h = diag(r) * x;
    q_hat = (x.' * x) / t;
    o_hat = (h.' * h) / t;

    l = round(ratio * t,0);

    for i = 1:(l - 1)
        o_tmp = (h(1:(t-i),:).' * h((1+i):t,:)) / (t - i);
        o_hat = o_hat + (((l - i) / l) * (o_tmp + o_tmp.'));
    end

    c = linsolve(q_hat,o_hat) / q_hat;

end

function [pval,pval_robust] = linear_granger_causality(in,out)

    t = length(in);
    y = out(2:t,1);
    x = [out(1:t-1) in(1:t-1)];

    [b,c,r] = hac_regression(y,x,0.1);

    xxi = inv(x.' * x);
    s2 = (r.' * r) / (t - 3);
    t_coefficients = b(2) / sqrt(s2 * xxi(2,2));

    pval = 1 - normcdf(t_coefficients);
    pval_robust = 1 - normcdf(b(2) / sqrt(c(2,2) / (t - 1)));

end

function r = validate_input(r)

    [t,n] = size(r);

    if ((t < 5) || (n < 2))
        error('The value of ''r'' is invalid. Expected input to be a matrix with a minimum size of 5x2.');
    end

end


%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[varargout]=deg180(varargin)
%DEG180  Converts degrees to the range [-180,180].
%
%   [D1,D2,...,DN]=DEG180(D1,D2,...,DN) converts the input angles, which
%   are measured in degrees, to the range [-180, 180].
%
%   DEG180 also works if D1,D2,...,DN are cell arrays of numerical arrays.
%
%   See also JDEG2RAD, JRAD2DEG, DEG180, DEGUNWRAP.
%
%   'deg180 --t' runs a test.
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2007--2015 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1}, '--t')
    deg180_test,return
end

varargout=varargin;
for i=1:nargin;
    if ~iscell(varargin{1})
        varargout{i}=deg360(varargin{i});
        bool=varargout{i}>180;
        varargout{i}(bool)=varargout{i}(bool)-360;
    else
        for j=1:length(varargin{1})
            varargout{i}{j,1}=deg360(varargin{i}{j});
            bool=varargout{i}{j}>180;
            varargout{i}{j}(bool)=varargout{i}{j}(bool)-360;
        end
    end
    %varargout{i}=angle(exp(sqrt(-1)*varargin{i}*2*pi/360))*360/2/pi;
    %varargout{i}(~isfinite(varargin{i}))=varargin{i}(~isfinite(varargin{i}));
    %    varargout{i}=jrad2deg(jdeg2rad(varargin{i})); Same but slower
end

function[]=deg180_test
thi=[359 181 nan inf];
tho=[-1 -179 nan inf];
tol=1e-10;
reporttest('DEG180 simple',aresame(deg180(thi),tho,tol))


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 [ y ] = FFT3D( x )
%FFT3D Summary of this function goes here
%   Detailed explanation goes here

y = fftshift(fftn(ifftshift(x)));

end


function jac = p01_jac ( option, nvar, x )

%*****************************************************************************80
%
%% P01_JAC evaluates the jacobian for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Input, integer NVAR, the number of variables.
%
%    Input, real X(NVAR), the argument of the jacobian.
%
%    Output, real JAC(NVAR-1,NVAR), the jacobian matrix evaluated
%    at X.  The NVAR-th row is not set by this routine.
%
  jac = zeros ( nvar, nvar );

  jac(1,1) = 1.0;
  jac(2,1) = 1.0;

  jac(1,2) = ( - 3.0 * x(2) + 10.0 ) * x(2) -  2.0;
  jac(2,2) = (   3.0 * x(2) +  2.0 ) * x(2) - 14.0;
%
%  Get the starting point
%
  y = p01_start ( option, nvar );
%
%  Get the function value at the starting point
%
  gy = p01_gx ( y );

  jac(1,3) = gy(1);
  jac(2,3) = gy(2);

  return
end


function [Cxy, f] = mycohere(varargin)
%COHERE Coherence function estimate.
%   Cxy = COHERE(X,Y,NFFT,Fs,WINDOW) estimates the coherence of X and Y
%   using Welch's averaged periodogram method.  Coherence is a function
%   of frequency with values between 0 and 1 that indicate how well the
%   input X corresponds to the output Y at each frequency.  X and Y are
%   divided into overlapping sections, each of which is detrended, then
%   windowed by the WINDOW parameter, then zero-padded to length NFFT.
%   The magnitude squared of the length NFFT DFTs of the sections of X and
%   the sections of Y are averaged to form Pxx and Pyy, the Power Spectral
%   Densities of X and Y respectively. The products of the length NFFT DFTs
%   of the sections of X and Y are averaged to form Pxy, the Cross Spectral
%   Density of X and Y. The coherence Cxy is given by
%       Cxy = (abs(Pxy).^2)./(Pxx.*Pyy)
%   Cxy has length NFFT/2+1 for NFFT even, (NFFT+1)/2 for NFFT odd, or NFFT
%   if X or Y is complex. If you specify a scalar for WINDOW, a Hanning
%   window of that length is used.  Fs is the sampling frequency which does
%   not effect the cross spectrum estimate but is used for scaling of plots.
%
%   [Cxy,F] = COHERE(X,Y,NFFT,Fs,WINDOW,NOVERLAP) returns a vector of freq-
%   uencies the same size as Cxy at which the coherence is computed, and
%   overlaps the sections of X and Y by NOVERLAP samples.
%
%   COHERE(X,Y,...,DFLAG), where DFLAG can be 'linear', 'mean' or 'none',
%   specifies a detrending mode for the prewindowed sections of X and Y.
%   DFLAG can take the place of any parameter in the parameter list
%   (besides X and Y) as long as it is last, e.g. COHERE(X,Y,'mean');
%
%   COHERE with no output arguments plots the coherence in the current
%   figure window.
%
%   The default values for the parameters are NFFT = 256 (or LENGTH(X),
%   whichever is smaller), NOVERLAP = 0, WINDOW = HANNING(NFFT), Fs = 2,
%   P = .95, and DFLAG = 'none'.  You can obtain a default parameter by
%   leaving it off or inserting an empty matrix [], e.g.
%   COHERE(X,Y,[],10000).
%
%   See also PSD, CSD, TFE.
%   ETFE, SPA, and ARX in the Identification Toolbox.

%   Author(s): T. Krauss, 3-31-93
%   Copyright (c) 1988-98 by The MathWorks, Inc.
%   $Revision: 1.1 $  $Date: 1998/06/03 14:42:19 $

narginchk(2,7);
x = varargin{1};
y = varargin{2};
%[msg,nfft,Fs,window,noverlap,p,dflag]=psdchk(varargin(3:end),x,y);
%error(msg)
nfft = 256;
Fs =8000;
window=hanning(nfft);
noverlap = 0.75*nfft;
p = .95;
dflag = 'none';

% compute PSD and CSD
window = window(:);
n = length(x);		% Number of data points
nwind = length(window); % length of window
if n < nwind    % zero-pad x , y if length is less than the window length
    x(nwind)=0;
    y(nwind)=0;
    n=nwind;
end
x = x(:);		% Make sure x is a column vector
y = y(:);		% Make sure y is a column vector
k = fix((n-noverlap)/(nwind-noverlap));	% Number of windows
					% (k = fix(n/nwind) for noverlap=0)
index = 1:nwind;

Pxx = zeros(nfft,1); Pxx2 = zeros(nfft,1);
Pyy = zeros(nfft,1); Pyy2 = zeros(nfft,1);
Pxy = zeros(nfft,1); Pxy2 = zeros(nfft,1);
for i=1:k
    if strcmp(dflag,'none')
        xw = window.*x(index);
        yw = window.*y(index);
    elseif strcmp(dflag,'linear')
        xw = window.*detrend(x(index));
        yw = window.*detrend(y(index));
    else
        xw = window.*detrend(x(index),0);
        yw = window.*detrend(y(index),0);
    end
    index = index + (nwind - noverlap);
    Xx = fft(xw,nfft);
    Yy = fft(yw,nfft);
    Xx2 = abs(Xx).^2;
    Yy2 = abs(Yy).^2;
    Xy2 = Yy.*conj(Xx);
    Pxx = Pxx + Xx2;
    Pxx2 = Pxx2 + abs(Xx2).^2;
    Pyy = Pyy + Yy2;
    Pyy2 = Pyy2 + abs(Yy2).^2;
    Pxy = Pxy + Xy2;
    Pxy2 = Pxy2 + Xy2.*conj(Xy2);
end

% Select first half
if ~any(any(imag([x y])~=0)),   % if x and y are not complex
    if rem(nfft,2),    % nfft odd
        select = [1:(nfft+1)/2];
    else
        select = [1:nfft/2+1];   % include DC AND Nyquist
    end
    Pxx = Pxx(select);
    Pxx2 = Pxx2(select);
    Pyy = Pyy(select);
    Pyy2 = Pyy2(select);
    Pxy = Pxy(select);
    Pxy2 = Pxy2(select);
else
    select = 1:nfft;
end
%Coh = (abs(Pxy).^2)./(Pxx.*Pyy);             % coherence function estimate
Coh = Pxy./sqrt(Pxx.*Pyy);
freq_vector = (select - 1)'*Fs/nfft;

% set up output parameters
if (nargout == 2),
   Cxy = Coh;
   f = freq_vector;
elseif (nargout == 1),
   Cxy = Coh;
elseif (nargout == 0),   % do a plot
   newplot;
   plot(freq_vector,Coh), grid on
   xlabel('Frequency'), ylabel('Coherence Function Estimate');
end


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


function [Ydt, X, R] = detrend_data(Y,nk, method)
%% detrend fluorescence signals with B-spline basis
%% Inputs:
%   Y: d X T matrix, video data
%   nk: scalar, number of knots
%   method: {'local_min', 'spline'} method for removing the trend
% Outputs:
%   Ydt: d X T matrix, detrended data
%   X: T*M matrix, each column is one basis
%   R: d*M matrix, coefficients of all basis for all pixels

%% create basis
[~, T] = size(Y);

if ~exist('nk', 'var')
    nk = 5;
end
if ~exist('method', 'var') || isempty(method)
    method = 'spline';
end

if strcmpi(method, 'spline')
    X = bsplineM((1:T)', linspace(1, T, nk), 4);

    %% compute coefficients of all spline basis
    R = (Y*X)/(X'*X);

    %% compute detrended data
    Ydt = Y-R*X';
else
    k = ceil(T/nk);
    [d, T] = size(Y);
    Tnew = ceil(T/k)*k;
    if T~=Tnew
        Y(:, (T+1):Tnew) = repmat(Y(:, T), [1, Tnew-T]);
    end
    Y = reshape(Y, d, k, []);
    Ydt = reshape(bsxfun(@minus, Y, min(Y,[], 2)), d, []);
    Ydt = Ydt(:, 1:T);
    X = [];
    R = [];
end


function [res,ires]=dydt1(t,y,yp)

res(1)=yp(1)+4*y(1);
res(2)=yp(2)+6*y(2);
res(3)=y(3)-y(1)-2.0;
ires=0;


% 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 [P, Q] = invMyForm(A, B, C)
    % return inv(myForm(A, B, C))
    % -----------------------------------------------
    % Author: Tiep Vu, thv102@psu.edu, 4/8/2016
    %         (http://www.personal.psu.edu/thv102/)
    % -----------------------------------------------
    if nargin == 0
        d = 10;
        C = 10;
        A = rand(d, d);
        B = rand(d, d);
        M = myForm(A, B, C);
    end
    %%
    invA = inv(A);
    invB = inv(B);
    % Q = myForm(invA, -invA*inv(invB + C*invA)*invA, C);
    P = invA;
    Q = -invA*inv(invB + C*invA)*invA;
    % toc;
    if nargin == 0
        M1 = inv(M);
        M2 = myForm(P, Q, C);
        norm(M1 - M2)
        pause
    end
end


function [M, rng] = intRescale(M, type);
%
% [M, rng] = intRescale(M, <type = int16>);
%
% Take a double or other numeric matrix,
% rescale to the range of the specified
% data type (by default 'int16') and return
% M in that type, as well as the range
% [min max] in M before scaling. This
% is intended to be used for dealing
% with large data sets (e.g., time series
% from many scans) which take up tons of
% memory as doubles.
%
% To convert M back to its double form,
% use:
%       M = normalize(M, rng(1), rng(2));
%
% Note that this will introduce slight inaccuracies
% in the reconstructed matrix (see example below). These
% are usually ~1/1000 of the actual values, and if the
% stored numbers are integers, rounding will resolve this.
% But, it can be an issue for applications which require
% great precision (greater than the number of integers in
% the int16 type).
%
% EXAMPLE:
%   [new, rng]  = intRescale(1:10);
%   old = normalize(new, rng(1), rng(2));
%
% ras, 06/05.
if ~exist('type','var') | isempty(type),
    type = 'int16';
end

rng = double([min(M(:)) max(M(:))]);

% get rescaled min/max, based on data type:
% there are functions for this in ML7, but
% in earlier matlab we have to kludge it:
v = version;
if str2num(v(1))>7
    resMin = intmin(type);
    resMax = intmax(type);
else
    resMin = -32768;
    resMax = 32768;
end

if diff(rng)==0
    % if the requested range is 0 (min==max), then we just need to remove the
    % current offset (=rng(1), which also =rng(2)) and apply the new scale
    % and offset.
    M = round((M-rng(1)) * (resMax-resMin) + resMin);
else
    M = round( (M-rng(1)) ./ diff(rng) * (resMax-resMin) + resMin );
end

cmd = sprintf('M = %s(M);',type);
eval(cmd);

return


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')


clear all; close all;
I=imread('cameraman.tif');
fun1=@dct2;
J1=blkproc(I, [8 8], fun1);
fun2=@(x) std2(x)*ones(size(x));
J2=blkproc(I, [8 8], fun2);
figure;
subplot(121);
imagesc(J1);
subplot(122);
imagesc(J2);
colormap gray;


function [L, Lratio, df] = L_Ratio(Fet, ClusterSpikes, m)

% [L, Lratio] = L_Ratio(Fet, ClusterSpikes)
%
% L-ratio
% Measure of cluster quality
%
% Inputs:   Fet:           N by D array of feature vectors (N spikes, D dimensional feature space)
%           ClusterSpikes: Index into Fet which lists spikes from the cell whose quality is to be evaluated.
%           m:             squared mahalanobis distances, default is to
%                          calculate them directly

% find # of spikes in this cluster
if nargin < 3
	nSpikes = size(Fet,1);
else
	nSpikes = size(m,1);
end

nClusterSpikes = length(ClusterSpikes);
if nClusterSpikes < size(Fet,2)
    L = nan;
    Lratio = nan;
    df = nan;
    warning('more features and spikes, L-Ratio is NaN...')
    return
end
% mark spikes which are not cluster members
NoiseSpikes = setdiff(1:nSpikes, ClusterSpikes);

%%%%%%%%%%% compute mahalanobis distances %%%%%%%%%%%%%%%%%%%%%
if nargin < 3
	m = mahal(Fet, Fet(ClusterSpikes,:));
end

mCluster = m(ClusterSpikes); % mahal dist of spikes in the cluster
mNoise = m(NoiseSpikes); % mahal dist of all other spikes

df = size(Fet,2);

L = sum(1-chi2cdf(m(NoiseSpikes),df));
Lratio = L/nClusterSpikes;


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 Rank=ks(X)
%+++ Employ the K-S algorithm for selecting the representative samples;
%+++ Basic idea of KS: Find from the candidates the sample whose minimal
%    distance from the samples in representative sets is the maximal.
%+++ Hongdong Li, May 10,2008.
%+++ Revised in Dec. 5, 2009.

tic;
[Mx,Nx]=size(X);
Rank=zeros(1,Mx);           %+++ Record the indices of the representative samples.
out=1:Mx;
D=distli(X);
[i j]=find(D==max(max(D)));
Rank(1)=i(1);               %+++ Initializes as the two samples of the furthest distance.
Rank(2)=j(1);
out([i(1) j(1)])=[];        %+++ The remaining samples.
%+++ Iteration of  K-S algorithm %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
iter=3;
while iter<=Mx
   in=Rank(find(Rank>0));
   Dsub=D(in,out);
   [minD,indexmin]=min(Dsub);
   [maxD,indexmax]=max(minD);
   Vadd=out(indexmax);
   Rank(iter)=Vadd;
   out(find(out==Vadd))=[];
   iter=iter+1;
end
toc;
%+++ Iteration ended %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%% END OF SUB
function D=distli(X)
X=X';
[D,N] = size(X);
X2 = sum(X.^2,1);
D = repmat(X2,N,1)+repmat(X2',1,N)-2*X'*X;


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 pass = test_partitionCombine( )
% Test the partition and combine methods.

tol = 1e3*chebfunpref().cheb2Prefs.chebfun2eps;

% Check that partitioning an empty spherefun gives two empty spherefuns
f = spherefun;
[feven,fodd] = partition(f);
pass(1) = isempty(feven) & isempty(fodd);

feven = spherefun;
fodd = spherefun;
f = combine(feven,fodd);
pass(2) = isempty(f);

% Check that an even spherefun is partitioned correctly.
f = spherefun(@(x,y,z) sin(pi*x.*y));  % Strictly even/pi-periodic
[feven,fodd] = partition(f);
pass(3) = isequal(feven,f);
pass(4) = isempty(fodd);

% Check that odd spherefun is partitioned correctly.
f = spherefun(@(x,y,z) sin(pi*x.*z));  % Strictly odd/anti-periodic
[feven,fodd] = partition(f);
pass(5) = isequal(fodd,f);
pass(6) = isempty(feven);

% Check that the even and odd terms are partitioned correctly
fe = @(x,y,z) sin(pi*x.*y);  % Strictly even/pi-periodic
fo = @(x,y,z) sin(pi*x.*z);  % Strictly odd/anti-periodic
f = spherefun(@(x,y,z) fe(x,y,z) + fo(x,y,z));
[feven,fodd] = partition(f);
pass(7) = norm(spherefun(fe)-feven) < tol;
pass(8) = norm(spherefun(fo)-fodd) < tol;

% Check that combine puts an even spherefun and empty spherfun back
% together.
feven = spherefun(@(x,y,z) sin(pi*x.*y));  % Strictly even/pi-periodic
fodd = spherefun;
f = combine(feven,fodd);
pass(9) = norm(feven-f) < tol;

% Check that combine puts an even spherefun and empty spherfun back
% together.
feven = spherefun;
fodd = spherefun(@(x,y,z) sin(pi*x.*z));  % Strictly odd/anti-periodic
f = combine(feven,fodd);
pass(10) = norm(fodd-f) < tol;

% Check that combine puts non-empty odd and even spherefuns back together.
fe = @(x,y,z) sin(pi*x.*y);  % Strictly even/pi-periodic
fo = @(x,y,z) sin(pi*x.*z);  % Strictly odd/anti-periodic
fcombine = spherefun(@(x,y,z) fe(x,y,z) + fo(x,y,z));
f = combine(spherefun(fe),spherefun(fo));
pass(11) = norm(fcombine-f) < tol;

% Check that combine cannot put together spherefuns that are not strictly
% odd or even.
try
    f = spherefun(@(x,y,z) sin(pi*x.*y) + sin(pi*x.*z));
    g = combine(f,f);
    pass(12) = false;
catch ME
    pass(12) = strcmp(ME.identifier, 'CHEBFUN:SPHEREFUN:combine:parity');
end

end


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


%data=Ka^ind+M;
%data must start from zero (K+M, Ka+M,....)
%data boun is the first and final ind values to be used (can be 0:end)

%ctyp: the method used to compute expoenential
%ctyp==0 => log based least square (all values must be postivie)
%ctyp==1 => normal equation (can be used also for negative values)

function [dmar_par exp_par]=expo_fit(dboun, data, ctyp, show_res, cval)
[ndim ndat]=size(data);
if (ctyp==2)
    nbas=size(cval,2);
else
    nbas=1;
end
a=zeros(ndim,nbas);
M=zeros(ndim,1);
K=zeros(ndim,nbas);

for i=1:ndim
    lb=dboun(i,1);
    ub=dboun(i,2);

    if (ctyp==1)
        %%normal equations based computation
        sr_a=data(i,(lb+1):ub)*data(i,(lb+1):ub)';
        sr_b=data(i,(lb+1):ub)*data(i,(lb+2):ub+1)';
        a_apx=sr_b/sr_a;
        dind=lb:ub;
    elseif (ctyp==0)
        %%apply a log based least square to compute a
        ind=(lb:ub);
        pind=find(data(i,ind+1)>eps*100);
        dind=ind(pind);

        H=[ones(length(dind),1) dind'];
        Y=log(data(i,dind+1))';
        x=inv(H'*H)*H'*Y;
        K_apx0=exp(x(1));
        a_apx=exp(x(2));
    elseif (ctyp==2)    %exponential bases are provided
        a_apx=cval(i,:);
        dind=lb:ub;
    end

    %optimal K & M given suboptimal "a"
    H=zeros(length(dind),nbas+1);
    for k=1:nbas
        H(:,k)=a_apx(k).^dind;
    end
    H(:,nbas+1)=1;

    apx_sol=inv(H'*H)*H'*data(i,dind+1)';
    K_apx=apx_sol(1:nbas)';
    M_apx=apx_sol(nbas+1);

    a(i,:)=a_apx;
    K(i,:)=K_apx;
    M(i)=M_apx;

    %compare the results
    if (show_res==1)
        figure;
        %compare the results
        ind=0:(length(data(i,:))-1);
        plot(ind,data(i,:));grid;
        hold on
        val_tot=0;
        for k=1:nbas
            val_th=K(i,k)*a(i,k).^ind+M(i);
            val_tot=val_tot+val_th-M(i);
            plot(ind,val_th,'r');
        end
        plot(ind,val_tot+M(i),'r');
    end
end

%exponential param
exp_par=[a K M];

%discrete markov param x'=ax+bu
b=(K.*(ones(ndim,1)-a.^2)).^0.5;
dmar_par=[a b];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     %optimal solution given suboptimal "a"
%     vr_a=a_apx.^dind;
%     sr_a=sum(vr_a);
%     sr_b=data(i,dind+1)*vr_a';
%     sr_c=sum(vr_a.^2);
%     sr_d=sum(data(i,dind+1));
%     sr_e=length(dind);
%
%     sr_f=sr_a^2-sr_e*sr_c;
%     M_apx=(sr_a*sr_b-sr_c*sr_d)/sr_f;
%     K_apx=(sr_a*sr_d-sr_b*sr_e)/sr_f;

%     %apply curve fit
%     %fit function
%     myfun2=@(p,k) p(1)+p(2)*p(3).^k;
%     opt_param=lsqcurvefit(myfun2,[M_apx, K_apx, a_apx],ind,data(i,:));
%     a(i)=opt_param(3);
%     K(i)=opt_param(2);
%     M(i)=opt_param(1);


function [ dz ] = odesys( ~,z,param )
% Copyright 2011 The MathWorks, Inc.
% Uses natural frequency (wn) and damping ratio (zeta) to form state
% equations for a standard 2nd order system.  The time derivative of
% the state vector is:

dz = [0 1; -param.wn^2 -2*param.zeta*param.wn]*z;

end


function [WRA, WF, WR] = compute_SWTFiltResp(params)
%% Load parameters
rs = params.rs;
cs = params.cs;
nlev = params.Wavelet.nlev;

lod = params.Wavelet.lod; %% Normalize filter for easier SWT implementation
hid = params.Wavelet.hid; %% Normalize filter for easier SWT implementation

% Get the shift-invariant wavelet filters
indicator = zeros(rs, cs);
indicator(1,1) = 1;
SWC = myswt2(indicator, nlev, lod, hid);

WF = zeros(rs, cs, 3*nlev+1);
WR = WF;
WRA = WR;

for ib = 1:3
    for ilev = 1:nlev
        WF(:,:,(ib-1)*nlev+ilev) = SWC(:,:,(ib-1)*nlev+ilev);
        WR(:,:,(ib-1)*nlev+ilev) = fft2(WF(:,:,(ib-1)*nlev+ilev));
        WRA(:,:,(ib-1)*nlev+ilev) = abs(WR(:,:,(ib-1)*nlev+ilev)).^2;
    end
end
WF(:,:,3*nlev+1) = SWC(:,:,3*nlev+1);
WR(:,:,3*nlev+1) = fft2(WF(:,:,3*nlev+1));
WRA(:,:,3*nlev+1) = abs(WR(:,:,3*nlev+1)).^2;

% DCR = 0;
% for ib = 1:3*nlev
%     DCR = DCR + WRA(:,:,ib);
% end
% dif = DCR-ACR;
% max(abs(dif(:)))


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