function [logp, s] = kde(train, test, s)

%KDE Kernel Density Estimation.
%    This function computes a kernel density estimator from a set of examples,
%    by placing Gaussian kernels (with identical masses) on each training data
%    point and adjusting the "widths" to maximize the sum of leave-one-out log
%    densities. In multivariate data, either isotropic or diagonal covariance
%    matrix Gaussians are used.
%
%    usage: [logp, s] = kde(train, test, s)
%
%    inputs:  train (n by D) is a matrix of n training points in D dimensions
%             test  (N by D) is a matrix of test points
%             s     scalar or (1 by D) is a start guess for std devs (optional)
%
%    outputs: logp  (1 by N) is the vector of test log densities
%             s     final std devs
%
%    The size of the initial guess for s indicates whether isotropic or
%    diagonal covariance is desired. If no initial guess is supplied, diagonal
%    is assumed. The method uses a Newton scheme in the log of s, and usually
%    converges in very few iterations. The Newton steps are checked to ensure
%    that a reasonable fraction (half) of the expected improvement is achieved;
%    otherwise smaller steps are tried. The computational complexity is order
%    (nD)^2 + nND, memory requirement order Dn(N+n). The algorithm may
%    encounter numerical problems if initial guess is too small.
%
%    (C) Copyright Carl Edward Rasmussen, July 4th 2000.

[N, D] = size(test); [n, D] = size(train);  % get number of cases and dimension
if nargin == 2                                % if no start guess is given then
  s = std(train)/(n^(0.5/D));      % use scaled empirical axis aligned std devs
end
P = length(s);                                    % number of parameters to fit

t = repmat(train,[1,1,n]);
if P == 1                                    % if we are fitting a single width
  c = sum((permute(t,[1,3,2])-permute(t,[3,1,2])).^2,3);
else                                                 % else multiple parameters
  c = (permute(t,[1,3,2])-permute(t,[3,1,2])).^2;
end

G = 1; TINY = 1e-10;
ec = exp(-sum(c./repmat(permute(2*s.^2,[1,3,2]),[n,n,1]),3));
secd = repmat(sum(ec-eye(n),2),[1,P]);
f_old = sum(log(secd(:,1)))-n*sum(log(s));
while max(abs(G)) > TINY
  x = shiftdim(sum(repmat(ec,[1,1,P]).*c,1))./secd;
  DE = sum(x,1)./s.^2;
  xx = repmat(x,[1 1 P]);
  for i=1:P        % rewriting this loop as matrix expr would cost too much mem
    DDE(i,1:P) = sum(shiftdim(sum(c.*repmat(ec.*c(:,:,i),[1,1,P])))./secd);
  end
  DDE = (DDE - shiftdim(sum(xx.*permute(xx,[1,3,2]))))./(s'*s).^2;
  [v, l] = eig(DDE-2*diag(DE));                               % eigs of Hessian
  l = max(abs(diag(l)),min(l(:)/TINY));    % control sign and magnitude of eigs
  G = (n*D/P-DE)*v*diag(-1./l)*v';                        % compute Newton step
  G = G/max(1, sqrt(G*G'));                       % don't take too large a step
  eta = 1; s_old = s;
  while eta == 1 | f_new < f_old - eta*G*(n*D/P-DE')/2 - TINY    % improvement?
    s = s_old.*exp(G*eta);
    eta = eta/2;           % if we fail, then try smaller step next time around
    ec = exp(-sum(c./repmat(permute(2*s.^2,[1,3,2]),[n,n,1]),3));
    secd = repmat(sum(ec-eye(n),2),[1,P]);
    f_new = sum(log(secd(:,1)))-n*sum(log(s));
  end
  f_old = f_new;                   % remember function value for next iteration
end

x = repmat(2*s.^2,[n,D/P]);
logp = zeros(N, 1);
for i=1:N          % rewriting this loop as matrix expr would cost too much mem
  cc = sum((repmat(test(i,:),[n,1])-train).^2./x,2);
  hh = min(cc);
  logp(i) = log(mean(exp(hh-cc)))-hh;
end

logp = logp - D*log(2*pi)/2 - D*sum(log(s))/P;            % normalize densities


function out = trigcoeffs(f, N)
%TRIGCOEFFS   Trigonometric Fourier coefficients of a CLASSICFUN.
%   C = TRIGCOEFFS(F) returns the trigonometric Fourier coefficients of F
%   using complex-exponential form.  Specifically, for N = length(F)
%   If N is odd
%       F(x) = C(1)*z^(N-1)/2 + C(2)*z^((N-1)/2-1) + ... + C((N+1)/2) + ...
%                + C(N)*z^(-(N-1)/2)
%   If N is even
%       F(x) = C(1)*z^(N/2-1) + C(2)*z^(N/2-2) + ... + C(N/2) + ...
%                + C(N-1)*z^(-N/2-1) + 1/2*C(N)*(z^(N/2) + z^(-N/2))
%   where z = exp(1i*pi*x).
%
%   A = TRIGCOEFFS(F, N) truncates or pads the vector C so that N coefficients
%   of F are returned.
%
%   If F is array-valued with M columns, then C is an MxN matrix.
%
% See also LEGCOEFFS, CHEBCOEFFS.

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

if ( nargin == 1 )
    N = length(f);
end

% Call TRIGCOEFFS() of the .ONEFUN:
out = trigcoeffs(f.onefun, N);

end


%  Figure 3.4      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
% script to generate Fig. 3.4
%% fig3_04.m    Example 3.5
clf;
k=1;
num=1;                        % form numerator
den=[1 k];                    % form denominator
% sinusoidal input signal
deltaT = 0.001;
t=0:deltaT:10;                % form time vector
u=sin(10*(t));                % form input
sys=tf(num,den);              % form system
[y]=lsim(sys,u,t);            % linear simulation
% plot response
figure();
plot(t,y);
xlabel('Time (sec)');
ylabel('Output');
title('Fig. 3.4 (a): transient response');
pause;
hold on;
y1=(10/101)*exp(-t);
phi=atan(-10);
y2=(1/sqrt(101))*sin(10*t+phi);
plot(t,y1,t,y2,t,y1+y2);
% grid
nicegrid
hold off;
pause;
figure();
ii=[9001:10001];
plot(t(ii),y(ii),t(ii),u(ii));
xlabel('Time (sec)');
ylabel('Output, input');
title('Fig. 3.4 (b): Steady-state response');
text(9.4,0.65,'u(t)');
text(9.24,0.12,'y(t)');
% grid
nicegrid


classdef prtPreProcLda < prtPreProcClass
    % prtPreProcLda  Linear discriminant analysis processing
    %
    %   preProc = prtPreProcLda creates a linear discriminant pre
    %   processing object. A prtPreProcLda object projects the input data
    %   onto a linear space that best separates class labels
    %
    %   A prtPreProcLda object has the following properties:
    %
    %   nComponents - The number of dimensions to project the data onto.
    %                 This must less than or equal to the input data's
    %                 number of features, and less than or equal to the
    %                 input data sets number of classes.
    %
    %   A prtPreProcLda object also inherits all properties and functions from
    %   the prtAction class
    %
    %   More information about LDA can be found at the following URL:
    %   http://en.wikipedia.org/wiki/Linear_discriminant_analysis
    %
    %   Example:
    %
    %   dataSet = prtDataGenIris;               % Load a dataset
    %   dataSet = dataSet.retainFeatures(1:3);  % Retain the first 3 features
    %   lda = prtPreProcLda;                    % Create the pre-processor
    %
    %   lda = lda.train(dataSet);               % Train
    %   dataSetNew = lda.run(dataSet);          % Run
    %
    %   % Plot the results
    %   subplot(2,1,1); plot(dataSet);
    %   title('Original Data');
    %   subplot(2,1,2); plot(dataSetNew);
    %   title('LDA Projected Data');
    %
    %   See Also: prtPreProc, prtPreProcPca, prtPreProcPls,
    %   prtPreProcHistEq, prtPreProcZeroMeanColumns, prtPreProcLda,
    %   prtPreProcZeroMeanRows, prtPreProcLogDisc, prtPreProcZmuv,
    %   prtPreProcMinMaxRows

    properties (SetAccess=private)
        name = 'Linear discriminant analysis' % Linear discriminant analysis
        nameAbbreviation = 'LDA' % LDA
    end

    properties
        nComponents = 2;   % The number of LDA components
    end
    properties (SetAccess=private)
        projectionMatrix = []; % The projection matrix
        globalMean = [];       % The global mean
    end

    methods

        % Allow for string, value pairs
        function Obj = prtPreProcLda(varargin)
            Obj = prtUtilAssignStringValuePairs(Obj,varargin{:});
        end
	end

	methods (Hidden = true)
        function featureNameModificationFunction = getFeatureNameModificationFunction(obj) %#ok<MANU>
            featureNameModificationFunction = prtUtilFeatureNameModificationFunctionHandleCreator('LDA Score #index#');
        end
	end

    methods
        function Obj = set.nComponents(Obj,nComp)
            if ~prtUtilIsPositiveScalarInteger(nComp)
                error('prt:prtPreProcPca','nComponents must be a positive scalar integer');
            end
            Obj.nComponents = nComp;
        end
    end

    methods (Access=protected,Hidden=true)

        function Obj = trainAction(Obj,DataSet)
            if Obj.nComponents > DataSet.nClasses
                error('prt:prtPreProcLda','Attempt to train LDA pre-processor with more components (%d) than unique classes in data set (%d)',Obj.nComponents,DataSet.nClasses);
            end
            [Obj.projectionMatrix,Obj.globalMean] = prtUtilLinearDiscriminantAnalysis(DataSet,Obj.nComponents);
        end

        function DataSet = runAction(Obj,DataSet)

            X = DataSet.getObservations;
            X = bsxfun(@minus,X,Obj.globalMean);
            DataSet = DataSet.setObservations(X*Obj.projectionMatrix);
        end

    end

end


function [reg_c,rho_c,eta_c] = l_corner(rho,eta,reg_param,U,s,b,method,M)
%L_CORNER Locate the "corner" of the L-curve.
%
% [reg_c,rho_c,eta_c] =
%        l_corner(rho,eta,reg_param)
%        l_corner(rho,eta,reg_param,U,s,b,method,M)
%        l_corner(rho,eta,reg_param,U,sm,b,method,M) ,  sm = [sigma,mu]
%
% Locates the "corner" of the L-curve in log-log scale.
%
% It is assumed that corresponding values of || A x - b ||, || L x ||,
% and the regularization parameter are stored in the arrays rho, eta,
% and reg_param, respectively (such as the output from routine l_curve).
%
% If nargin = 3, then no particular method is assumed, and if
% nargin = 2 then it is issumed that reg_param = 1:length(rho).
%
% If nargin >= 6, then the following methods are allowed:
%    method = 'Tikh'  : Tikhonov regularization
%    method = 'tsvd'  : truncated SVD or GSVD
%    method = 'dsvd'  : damped SVD or GSVD
%    method = 'mtsvd' : modified TSVD,
% and if no method is specified, 'Tikh' is default.  If the Spline Toolbox
% is not available, then only 'Tikh' and 'dsvd' can be used.
%
% An eighth argument M specifies an upper bound for eta, below which
% the corner should be found.

% Per Christian Hansen, IMM, July 26, 2007.

% Set default regularization method.
if (nargin <= 3)
  method = 'none';
  if (nargin==2), reg_param = (1:length(rho))'; end
else
  if (nargin==6), method = 'Tikh'; end
end

% Set this logical variable to 1 (true) if the corner algorithm
% should always be used, even if the Spline Toolbox is available.
alwayscorner = 0;

% Set threshold for skipping very small singular values in the
% analysis of a discrete L-curve.
s_thr = eps;  % Neglect singular values less than s_thr.

% Set default parameters for treatment of discrete L-curve.
deg   = 2;  % Degree of local smooting polynomial.
q     = 2;  % Half-width of local smoothing interval.
order = 4;  % Order of fitting 2-D spline curve.

% Initialization.
if (length(rho) < order)
  error('Too few data points for L-curve analysis')
end
if (nargin > 3)
  [p,ps] = size(s); [m,n] = size(U);
  beta = U'*b;
  if (m>n), b0 = b - U*beta; end
  if (ps==2)
    s = s(p:-1:1,1)./s(p:-1:1,2);
    beta = beta(p:-1:1);
  end
  xi = beta./s;
end

% Restrict the analysis of the L-curve according to M (if specified).
if (nargin==8)
  index = find(eta < M);
  rho = rho(index); eta = eta(index); reg_param = reg_param(index);
end

if (strncmp(method,'Tikh',4) | strncmp(method,'tikh',4))

  % The L-curve is differentiable; computation of curvature in
  % log-log scale is easy.

  % Compute g = - curvature of L-curve.
  g = lcfun(reg_param,s,beta,xi);

  % Locate the corner.  If the curvature is negative everywhere,
  % then define the leftmost point of the L-curve as the corner.
  [gmin,gi] = min(g);
  reg_c = fminbnd('lcfun',...
    reg_param(min(gi+1,length(g))),reg_param(max(gi-1,1)),...
    optimset('Display','off'),s,beta,xi); % Minimizer.
  kappa_max = - lcfun(reg_c,s,beta,xi); % Maximum curvature.

  if (kappa_max < 0)
    lr = length(rho);
    reg_c = reg_param(lr); rho_c = rho(lr); eta_c = eta(lr);
  else
    f = (s.^2)./(s.^2 + reg_c^2);
    eta_c = norm(f.*xi);
    rho_c = norm((1-f).*beta);
    if (m>n), rho_c = sqrt(rho_c^2 + norm(b0)^2); end
  end

elseif (strncmp(method,'tsvd',4) | strncmp(method,'tgsv',4) | ...
        strncmp(method,'mtsv',4) | strncmp(method,'none',4))

  % Use the adaptive pruning algorithm to find the corner, if the
  % Spline Toolbox is not available.
  if ~exist('splines','dir') | alwayscorner
    %error('The Spline Toolbox in not available so l_corner cannot be used')
    reg_c = corner(rho,eta);
    rho_c = rho(reg_c);
    eta_c = eta(reg_c);
    return
  end

  % Othersise use local smoothing followed by fitting a 2-D spline curve
  % to the smoothed discrete L-curve. Restrict the analysis of the L-curve
  % according to s_thr.
  if (nargin > 3)
    if (nargin==8)       % In case the bound M is in action.
      s = s(index,:);
    end
    index = find(s > s_thr);
    rho = rho(index); eta = eta(index); reg_param = reg_param(index);
  end

  % Convert to logarithms.
  lr = length(rho);
  lrho = log(rho); leta = log(eta); slrho = lrho; sleta = leta;

  % For all interior points k = q+1:length(rho)-q-1 on the discrete
  % L-curve, perform local smoothing with a polynomial of degree deg
  % to the points k-q:k+q.
  v = (-q:q)'; A = zeros(2*q+1,deg+1); A(:,1) = ones(length(v),1);
  for j = 2:deg+1, A(:,j) = A(:,j-1).*v; end
  for k = q+1:lr-q-1
    cr = A\lrho(k+v); slrho(k) = cr(1);
    ce = A\leta(k+v); sleta(k) = ce(1);
  end

  % Fit a 2-D spline curve to the smoothed discrete L-curve.
  sp = spmak((1:lr+order),[slrho';sleta']);
  pp = ppbrk(sp2pp(sp),[4,lr+1]);

  % Extract abscissa and ordinate splines and differentiate them.
  % Compute as many function values as default in spleval.
  P     = spleval(pp);  dpp   = fnder(pp);
  D     = spleval(dpp); ddpp  = fnder(pp,2);
  DD    = spleval(ddpp);
  ppx   = P(1,:);       ppy   = P(2,:);
  dppx  = D(1,:);       dppy  = D(2,:);
  ddppx = DD(1,:);      ddppy = DD(2,:);

  % Compute the corner of the discretized .spline curve via max. curvature.
  % No need to refine this corner, since the final regularization
  % parameter is discrete anyway.
  % Define curvature = 0 where both dppx and dppy are zero.
  k1    = dppx.*ddppy - ddppx.*dppy;
  k2    = (dppx.^2 + dppy.^2).^(1.5);
  I_nz  = find(k2 ~= 0);
  kappa = zeros(1,length(dppx));
  kappa(I_nz) = -k1(I_nz)./k2(I_nz);
  [kmax,ikmax] = max(kappa);
  x_corner = ppx(ikmax); y_corner = ppy(ikmax);

  % Locate the point on the discrete L-curve which is closest to the
  % corner of the spline curve.  Prefer a point below and to the
  % left of the corner.  If the curvature is negative everywhere,
  % then define the leftmost point of the L-curve as the corner.
  if (kmax < 0)
    reg_c = reg_param(lr); rho_c = rho(lr); eta_c = eta(lr);
  else
    index = find(lrho < x_corner & leta < y_corner);
    if ~isempty(index)
      [dummy,rpi] = min((lrho(index)-x_corner).^2 + (leta(index)-y_corner).^2);
      rpi = index(rpi);
    else
      [dummy,rpi] = min((lrho-x_corner).^2 + (leta-y_corner).^2);
    end
    reg_c = reg_param(rpi); rho_c = rho(rpi); eta_c = eta(rpi);
  end

elseif (strncmp(method,'dsvd',4) | strncmp(method,'dgsv',4))

  % The L-curve is differentiable; computation of curvature in
  % log-log scale is easy.

  % Compute g = - curvature of L-curve.
  g = lcfun(reg_param,s,beta,xi,1);

  % Locate the corner.  If the curvature is negative everywhere,
  % then define the leftmost point of the L-curve as the corner.
  [gmin,gi] = min(g);
  reg_c = fminbnd('lcfun',...
    reg_param(min(gi+1,length(g))),reg_param(max(gi-1,1)),...
    optimset('Display','off'),s,beta,xi,1); % Minimizer.
  kappa_max = - lcfun(reg_c,s,beta,xi,1); % Maximum curvature.

  if (kappa_max < 0)
    lr = length(rho);
    reg_c = reg_param(lr); rho_c = rho(lr); eta_c = eta(lr);
  else
    f = s./(s + reg_c);
    eta_c = norm(f.*xi);
    rho_c = norm((1-f).*beta);
    if (m>n), rho_c = sqrt(rho_c^2 + norm(b0)^2); end
  end

else
  error('Illegal method')
end


function dq = linevel2dq(v,r,vp,rp)

% LINEVEL2DQ     Transforms a line velocity expressed in vector notation
%                into its dual quaternion representation.
%
%      DQ = LINE2DQ(V,R,VP,RP) transforms the line position, specified by:
%          - the line orientation V
%          - the position of any point of the line, R.
%          - the line orientation rate of change, VP
%          - the velocity component (orthogonal to the line orientation V)
%          of point P, RP
%       V does not need to be unitary, but VP must be orthogonal to V. If
%       RP has a component in the V orientation, it does not matter, since
%       it does not change the expression of the resulting dual quaternion
%       DQ.
%       V,R,VP and RP must have the same size. The inputs (V,R,VP,RP) are
%       either a vector of size 3 or an array of size 3*N (column i
%       represents the input component of Line velocity i) where N is the
%       number of lines. DQ is either a vector of size 8, either an array
%       of size (8*N) depending on the input format. Each column of DQ
%       represents the dual quaternion representation of the corresponding
%       line position velocity.
%
% See also POS2DQ, VEL2DQ, LINE2DQ

sv = size(v);
sr = size(r);
svp = size(vp);
srp = size(rp);
if sv == [1 3],  v = v.'; sv = size(v); end
if sr == [1 3],  r = r.'; sr = size(r); end
if svp == [1 3],  vp = vp.'; svp = size(vp); end
if srp == [1 3],  rp = rp.'; srp = size(rp); end

% check that all inputs have the same size
tab_s = [sv; sr; svp; srp];
if max(tab_s) ~= min(tab_s)
      error('DualQuaternion:linevel2dquat:sizesDoNotMatch',...
        'Arrays v, r, vp and r should be the same size. Size of \n - v is [%d %d] \n - r is [%d %d] \n - vp is [%d %d] \n - rp is [%d %d]',...
           sv(1),sv(2),sr(1),sr(2),svp(1),svp(2),srp(1),srp(2));
end

% if the format is wrong
if sv(1) ~= 3
    error('DualQuaternion:linevel2dquat:wrongsize',...
        '%d rows in the V,R,VP and RP  arrays. It should be 3. ',sv(1));
end

% normalization of the axis vector (if necessary)
n = length(v(1,:));
n2 = sum(v.^2).^0.5;
n2 = repmat(n2,3,1);
v=v./n2;
vp=vp./n2;

% construction of the line velocity dual quaternion
dq = sym(zeros(8,n));
dq(2:4,:) = vp;
dq(6:8,:) = cross(r,vp)+cross(rp,v);


function cheby_u_poly_test ( )

%*****************************************************************************80
%
%% CHEBY_U_POLY_TEST tests CHEBY_U_POLY.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 January 2015
%
%  Author:
%
%    John Burkardt
%
  n_max = 12;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBY_U_POLY_TEST:\n' );
  fprintf ( 1, '  CHEBY_U_POLY evaluates the Chebyshev T polynomial.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N      X        Exact F       U(N)(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, x, fx ] = cheby_u_poly_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = cheby_u_poly ( 1, n, x );

    fprintf ( 1, '  %6d  %8f  %12f  %12f\n', n, x, fx, fx2(n+1) );

  end

  return
end


function [imgOut,SH] = DiffuseConvolutionSH(img, falloff)
%
%
%        [imgOut,SH]=DiffuseConvolutionSH(img,falloff)
%
%
%        Input:
%           -img: an environment map in the latitude-longitude mapping
%           -falloff: a flag. If it is set 1, it means that fall-off will
%                     be taken into account
%
%        Output:
%           -imgOut: a diffuse convolved version of img
%           -SH: a [3,9] vector where spherical harmonics for img are
%           encoded
%
%     Copyright (C) 2011  Francesco Banterle
%
%     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/>.
%

if(~exist('falloff', 'var'))
    falloff = 0;
end

%falloff compensation
if(falloff)
    img = FallOffEnvMap(img);
end

[r,c,col]=size(img);

SH = zeros(col, 9);

%projection constants
y00 = 0.282095;
y1x = 0.488603;
y2x = 1.092548;
y20 = 0.315392;
y22 = 0.546274;

%generation of directions

[X,Y] = meshgrid(1:c, 1:r);
phi   = pi * 2 * (X / c);
theta = pi * (Y / r);
sinTheta = sin(theta);

Dx = cos(phi) .* sinTheta;
Dy = cos(theta);
Dz = sin(phi) .* sinTheta;

for i=1:col
    img(:,:,i) = img(:,:,i) .* sinTheta;
end

%Environment projection on SH
for i=1:col
    %SH 0
    SH(i,1) = mean(mean(img(:,:,i) .* y00));
    %SH 1 -1 y
    SH(i,2) = mean(mean(img(:,:,i) .* Dy * y1x));
    %SH 1  0 z
    SH(i,3) = mean(mean(img(:,:,i) .* Dz * y1x));
    %SH 1  1 x
    SH(i,4) = mean(mean(img(:,:,i) .* Dx * y1x));
    %SH 2 -2 xy
    SH(i,5) = mean(mean(img(:,:,i) .* Dx .* Dy * y2x));
    %SH 2 -1 yz
    SH(i,6) = mean(mean(img(:,:,i) .* Dy .* Dz * y2x));
    %SH 2  1 xz
    SH(i,7) = mean(mean(img(:,:,i) .* Dx .* Dz * y2x));
    %SH 2  0 3z^2-1
    SH(i,8) = mean(mean(img(:,:,i) .* (3 * (Dz.^2) - 1) * y20));
    %SH 2  2 x^2-y^2
    SH(i,9) = mean(mean(img(:,:,i) .* (Dx.^2 - Dy.^2) * y22));
end

%scaling
SH = SH * pi * pi * 2;

%convolution
imgOut = EvaluationSH(SH, Dx, Dy, Dz);

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function [pathS1, pathS2] = MC_B_path(S0,r,d,sigmaB, sigmaBS,T,Z)
%
% Simulate a path within the Bachelier model model
%
%   S0 spot price
%   K the strike price
%   r the riskless rate
%   d the dividend yield
%   sigma the volatility
%   T the maturity
%   NTime number of timesteps
%   NSim number of simulations

NSim = size(Z,1);
NTime = size(Z,2);
Delta = T/NTime;                          % The discretisation step

lnS1 = zeros(NSim,NTime+1);               % init the logspot price path
lnS1(:,1)=log(S0*exp(-d*T));              % adjust due to dividend yield
S1 = zeros(NSim,NTime+1);
S1(:,1) = S0;

%dW = randn(NSim,NTime);                   % precompute all randoms

for i=1:NTime
    lnS1(:,i+1) = lnS1(:,i) + (r-d) * Delta + sigmaBS* sqrt(Delta)*Z(:,i);
    S1(:,i+1) = S1(:,i) + (r-d) * Delta + sigmaB * sqrt(Delta) * Z(:,i);
end

pathS1 = exp(lnS1);
pathS2 = S1;
clear dW;
end


function [CIJkcore,kn,peelorder,peellevel] = kcore_bu(CIJ,k)
%KCORE_BU       K-core
%
%   [CIJkcore,kn,peelorder,peellevel] = kcore_bu(CIJ,k);
%
%   The k-core is the largest subnetwork comprising nodes of degree at
%   least k. This function computes the k-core for a given binary
%   undirected connection matrix by recursively peeling off nodes with
%   degree lower than k, until no such nodes remain.
%
%   input:          CIJ,        connection/adjacency matrix (binary, undirected)
%                     k,        level of k-core
%
%   output:    CIJkcore,        connection matrix of the k-core.  This matrix
%                               only contains nodes of degree at least k.
%                    kn,        size of k-core
%                    peelorder, indices in the order in which they were
%                               peeled away during k-core decomposition
%                    peellevel, corresponding level - nodes at the same
%                               level were peeled away at the same time
%
%   'peelorder' and 'peellevel' are similar the the k-core sub-shells
%   described in Modha and Singh (2010).
%
%   References: e.g. Hagmann et al. (2008) PLoS Biology
%
%   Olaf Sporns, Indiana University, 2007/2008/2010/2012

%#ok<*AGROW>

peelorder = [];
peellevel = [];
iter = 0;

while 1

    % get degrees of matrix
    [deg] = degrees_und(CIJ);

    % find nodes with degree <k
    ff = find((deg<k)&(deg>0));

    % if none found -> stop
    if (isempty(ff)) break; end;            %#ok<SEPEX>

    % peel away found nodes
    iter = iter+1;
    CIJ(ff,:) = 0;
    CIJ(:,ff) = 0;

    peelorder = [peelorder; ff'];
    peellevel = [peellevel; iter.*ones(1,length(ff))'];

end;

CIJkcore = CIJ;
kn = sum(deg>0);


function [p,t,pfun,tfun] = fixmesh(p,t,pfun,tfun)

%*****************************************************************************80
%
%  FIXMESH: Ensure that triangular mesh data is consistent.
%
%  [p,t,pfun,tfun] = fixmesh(p,t,pfun,tfun);
%
%  p     : Nx2 array of nodal XY coordinates, [x1,y1; x2,y2; etc]
%  t     : Mx3 array of triangles as indices, [n11,n12,n13; n21,n22,n23;
%          etc]
%  pfun  : (Optional) NxK array of nodal function values. Each column in
%          PFUN corresponds to a dependent function, PFUN(:,1) = F1(P),
%          PFUN(:,2) = F2(P) etc, defined at the nodes.
%  tfun  : (Optional) MxK array of triangle function values. Each column in
%          TFUN corresponds to a dependent function, TFUN(:,1) = F1(T),
%          TFUN(:,2) = F2(T) etc, defined on the triangles.
%
% The following checks are performed:
%
%  1. Nodes not refereneced in T are removed.
%  2. Duplicate nodes are removed.
%  3. Triangles are ordered counter-clockwise.
%  4. Triangles with an area less than 1.0e-10*eps*norm(A,'inf')
%     are removed
%
%  Author:
%
%    Darren Engwirda
%

TOL = 1.0e-10;

if (nargin<4)
   tfun = [];
   if (nargin<3)
      pfun = [];
      if nargin<2
         error('Wrong number of inputs');
      end
   end
elseif (nargin>4)
   error('Wrong number of inputs');
end
if (nargout>4)
   error('Wrong number of outputs');
end
if (numel(p)~=2*size(p,1))
   error('P must be an Nx2 array');
end
if (numel(t)~=3*size(t,1))
   error('T must be an Mx3 array');
end
if (any(t(:))<1) || (max(t(:))>size(p,1))
   error('Invalid T');
end
if ~isempty(pfun)
   if (size(pfun,1)~=size(p,1)) || (ndims(pfun)~=2)
      error('PFUN must be an NxK array');
   end
end
if ~isempty(tfun)
   if (size(tfun,1)~=size(t,1)) || (ndims(tfun)~=2)
      error('TFUN must be an Mxk array');
   end
end

% Remove duplicate nodes
[i,i,j] = unique(p,'rows');
if ~isempty(pfun)
   pfun = pfun(i,:);
end
p = p(i,:);
t = reshape(j(t),size(t));

% Triangle area
A = triarea(p,t);
Ai = A<0.0;
Aj = abs(A)>TOL*norm(A,'inf');

% Flip node numbering to give a counter-clockwise order
t(Ai,[1,2]) = t(Ai,[2,1]);

% Remove zero area triangles
t = t(Aj,:);
if ~isempty(tfun)
   tfun = tfun(Aj,:);
end

% Remove un-used nodes
[i,j,j] = unique(t(:));
if ~isempty(pfun)
   pfun = pfun(i,:);
end
p = p(i,:);
t = reshape(j,size(t));

end      % fixmesh()


function [mod_res_apx exp_res_apx]=exp_approx2(dboun, exp_par1, exp_par2,show_res)
nexp=size(exp_par1,1);
exp_res_apx=zeros(nexp,3);
mod_res_apx=zeros(nexp,3);

for i=1:nexp
    dind=(dboun(i,1):dboun(i,2));

    Y=(exp_par1(i,1).^dind*exp_par1(i,2)-exp_par2(i,1).^dind*exp_par2(i,2))';
    p1=exp_par1(i,1);
    p2=exp_par2(i,1);
    sr_a=p1/(1-p1^2)/(p1-p2)/(1-p1*p2);
    sr_b=p2/(1-p2^2)/(p2-p1)/(1-p1*p2);
    H=[sr_a*p1.^dind'+sr_b*p2.^dind'];

    N_est=inv(H'*H)*H'*Y;

    exp_res_apx(i,:)=[p1,p2,sqrt(N_est)];
    mod_res_apx(i,:)=[p1+p2,-p1*p2,sqrt(N_est)];
end

if (show_res==1)
    for i=1:nexp
        dind=0:dboun(i,2);
        val_a=exp_par1(i,1).^dind*exp_par1(i,2);
        val_b=exp_par2(i,1).^dind*exp_par2(i,2);
        p1=exp_res_apx(i,1);
        p2=exp_res_apx(i,2);
        N=exp_res_apx(i,3)^2;
        sr_a=N*p1/(1-p1^2)/(p1-p2)/(1-p1*p2);
        sr_b=N*p2/(1-p2^2)/(p2-p1)/(1-p1*p2);
        val_c=sr_a*p1.^dind'+sr_b*p2.^dind';
        figure;
        plot(dind,val_a-val_b);grid;
        hold on
        plot(dind,val_c,'r');
    end
end


function [dists, wh] = canlab_fast_euclidean_distance(X, C, varargin)
% dists = canlab_fast_euclidean_distance(X, C)
%
% X = obs x variables (dimensions, n) for Set 1
% C = obs x variables (dimensions, n) for Set 2
%
% dists
%
% Adapted (largely just borrowed) from:
%%=========================================================
%  Fast Euclidean Distance Calculation
%
%   This script demonstrates the use of matrix multiplication to quickly
%   calculate the Euclidean distance between a large number of vectors.
%
% $Author: ChrisMcCormick $    $Date: 2014/08/22 22:00:00 $    $Revision: 1.0 $
%
% NOTE: (Tor):
% Dot product is SLOWER if n < 6, but gets dramatically faster
% with n = 6 or more. But in some tests it is about equally fast...
% Matrix with fewer observations should be C when using loop (sum sq diffs)
% approach.
%
%%=========================================================
%
% Optional arguments:
%
% 'tracktime'   Track and report time using both matrix and SSD method
% 'target_distance' Distance that counts as "close enough"
% 'squared_distance' Distance returned is squared - saves computation time
%                    Also assumes that target_distance is squared

dotracktime = true;
dosqrt = true;          % save more time by comparing squares to squared dist target
% don't do final sqrt if false, return squared distance

[m, n] = size(X);
k = size(C, 1);

%%  Sum-of-squared-differences approach
%%=========================================================

if dotracktime
    % Measure the time.
    tic();
end

% Create a matrix to hold the distances between each data point and
% each model.
dists = zeros(m, k);

% For each model...
for (i = 1 : k)

    % Subtract model i from all data points.
    diffs = bsxfun(@minus, X, C(i, :));

    % Take the square root of the sum of squared differences.
    dists(:, i) = sqrt(sum(diffs.^2, 2));
end

if dotracktime
    elapsed = toc();
    fprintf('Sum-of-squared-differences took %.3f seconds.\n', elapsed);
    if (exist('OCTAVE_VERSION')) fflush(stdout); end

    dists1 = dists;

end

%%=========================================================
%%  Matrix-multiply approach
%%=========================================================

if dotracktime
    tic();
end

% Calculate the sum of squares for all input vectors and
% for all cluster centers / models.
%
% Matrix dimensions:
%   X  [m  x  n]
%  XX  [m  x  1]
%   C  [k  x  n]
%  CC  [1  x  k]
XX = sum(X.^2, 2);
CC = sum(C.^2, 2)';

% Calculate the dot product between each input vector and
% each cluster center / model.
%
% Matrix dimensions:
%   X  [m  x  n]
%   C  [k  x  n]
%   C' [n  x  k]
%  XC  [m  x  k]
XC = X * C';

% Calculate the Euclidean distance between all input vectors in X
% and all clusters / models in C using the following equation:
%
%   z = sqrt(||x||^2 - 2xc' + ||c||^2)
%
%  Step 1: Subtract the column vector XX from every column of XC.
%  Step 2: Add the row vector CC to every row of XC.
%
% Matrix dimensions:
%     XX  [m  x  1]
%     XC  [m  x  k]
%     CC  [1  x  k]
%  dists  [m  x  k]
%
dists = sqrt(bsxfun(@plus, CC, bsxfun(@minus, XX, 2*XC)));

if dotracktime

    elapsed = toc();
    fprintf('Dot-product took %.3f seconds.\n', elapsed);

    dists2 = dists;

    % Make sure the resulting distances are nearly identical.
    fprintf('Difference in result: %f\n', sum(abs(dists1(:) - dists2(:))));
    if (exist('OCTAVE_VERSION')) fflush(stdout); end

end

end % function


function p = high_card_fun ( m, n )

%*****************************************************************************80
%
%% HIGH_CARD_FUN estimates the value of breaking the deck at location K.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of cards in the deck.
%
%    Input, integer N, the number of trials.
%
%    Output, real P(M), the estimated probability of picking the correct
%    high card by discarding so many cards and taking the next card that
%    is higher.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HIGH_CARD_FUN\n' );
  fprintf ( 1, '  Using N=%d cards and T=%d trials,\n', m, n );
  fprintf ( 1, '  estimate the chances of correctly picking the highest card\n' );
  fprintf ( 1, '  by looking at cards 0 through K-1, and then taking the first\n' );
  fprintf ( 1, '  subsequent card that is bigger.\n' );

  p = zeros ( m, 1 );

  parfor i = 1 : m

    p(i) = high_card_simulation ( m, n, i - 1 );

  end

  return
end
function p = high_card_simulation ( deck_size, trial_num, skip_num )

%*****************************************************************************80
%
%% HIGH_CARD_SIMULATION simulates a game of choosing the highest card in a deck.
%
%  Discussion:
%
%    You are given a deck of DECK_SIZE cards.
%
%    Your goal is to select the high card.  For convenience, we can assume
%    the cards are a permutation of the integers from 1 to DECK_SIZE, but in
%    fact the user mustn't see such values or else it's obvious which is the
%    largest card.
%
%    However, your choice is made under the following rules:  You may turn over
%    one card at a time.  When a card is turned over, you may declare that to be
%    your choice, or else turn over another card.  If you have not chosen a card
%    by the end, then your choice is the final card.
%
%    If you have no idea what to do, and simply decide in advance to pick
%    a card "at random", that is, for example, you decide to pick the 15th card
%    before having seen any cards, then your probability of winning is 1/DECK_SIZE.
%
%    The question is, can you do better than that?
%
%    Your strategy is as follows: always look at the first SKIP_NUM cards without
%    choosing them.  Then choose the very next card you encounter that is larger
%    than the cards you skipped.
%
%    Using this program, you can easily see that skipping 5 cards is much better
%    than picking one at random, skipping 10 is even better, and so on.  Of course,
%    you can't skip too many cards, and in fact, the results seem to be best for
%    somewhere around 30 to 35 cards skipped.  For problems like this, the
%    optimal value is somewhere around 1 / e, where E is the base of the natural
%    logarithm system.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DECK_SIZE, the number of cards in the deck.
%    2 <= DECK_SIZE.  Default value is 52;
%
%    Input, integer TRIAL_NUM, the number of times we will simulate this process.
%    Default value is 100.
%
%    Input, integer SKIP_NUM, the number of initial cards you plan to examine
%    but will NOT select.  If SKIP_NUM is 0, you don't look at any cards first.
%    0 <= SKIP_NUM < DECK_SIZE.  Default value is DECK_SIZE/3.
%
%    Output, real P, the estimated probability that your strategy of skipping
%    SKIP_NUM cards and then selecting the next card that is bigger, will
%    result in choosing the highest card.
%
  if ( nargin < 3 )
    trial_num = 100;
  end

  if ( nargin < 2 )
    skip_num = deck_size / 3;
  end

  if ( nargin < 1 )
    deck_size = 52;
  end
%
%  Make sure we got integers.
%
  deck_size = floor ( deck_size );
  skip_num = floor ( skip_num );
  trial_num = floor ( trial_num );
%
%  Check values.
%
  if ( deck_size < 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HIGH_CARD_SIMULATION - Fatal error!\n' );
    fprintf ( 1, '  DECK_SIZE must be at least 2.\n' );
    fprintf ( 1, '  Your value was %d\n', deck_size );
    error ( 'HIGH_CARD_SIMULATION - Fatal error!' );
  end

  if ( skip_num < 0 )
    skip_num = 0;
  end

  if ( deck_size <= skip_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HIGH_CARD_SIMULATION - Fatal error!\n' );
    fprintf ( 1, '  SKIP_NUM must be less than DECK_SIZE.\n' );
    fprintf ( 1, '  Your values were DECK_SIZE = %d, SKIP_NUM = %d\n', deck_size, skip_num );
    error ( 'HIGH_CARD_SIMULATION - Fatal error!' );
  end

  if ( trial_num < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HIGH_CARD_SIMULATION - Fatal error!\n' );
    fprintf ( 1, '  TRIAL_NUM must be at least 1.\n' );
    fprintf ( 1, '  Your value was %d\n', trial_num );
    error ( 'HIGH_CARD_SIMULATION - Fatal error!' );
  end

  correct = 0;

  for trial = 1 : trial_num

    cards = permutation_random ( deck_size );

    if ( 1 <= skip_num )
      skip_max = max ( cards(1:skip_num) );
    else
      skip_max = -Inf;
    end

    true_max = max ( cards(1:deck_size) );
%
%  In case you don't encounter a card larger than SKIP_MAX,
%  we'll assume you pick the last card in the deck, even though
%  you know it's a loser.
%
    choice = cards(deck_size);
%
%  Turn over the remaining cards in the deck, but stop
%  immediately when you find one bigger than SKIP_MAX.
%
    for card = skip_num + 1 : deck_size
      if ( skip_max < cards(card) )
        choice = cards(card);
        break;
      end
    end
%
%  Record successful choices.
%
    if ( choice == true_max )
      correct = correct + 1;
    end

  end
%
%  Estimate the probability.
%
  p = correct / trial_num;

  return
end
function p = permutation_random ( n )

%*****************************************************************************80
%
%% PERMUTATION_RANDOM returns a random permutation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of objects to permute.
%
%    Output, integer P(N), a permutation of the integers from 1 to N.
%
  p = ( 1 : n );

  for i = 1 : n - 1

    k = i4_random ( i, n );

    p1 = p(i);
    p(i) = p(k);
    p(k) = p1;

  end

  return
end
function value = i4_random ( lo, hi )

%*****************************************************************************80
%
%% I4_RANDOM returns a random integer between LO and HI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LO, HI, the limits.
%
%    Output, integer VALUE, the random integer.
%
  r = ( hi + 1 - lo ) * rand ( );
  value = lo + floor ( r );

  return
end


function [W,D,E] = cubic_winding_number(C,V,ispoly)
  % CUBIC_WINDING_NUMBER Cubic the winding number of points in V with respect to
  % a cubic Bezier curve in C
  %
  % [W,D,E] = cubic_winding_number(C,V)
  %
  % Inputs:
  %   C  4 by 2 list of control points
  %   V  #V by 2 list of query points
  % Outputs:
  %   W  #V list of winding number values
  %   D  #V list of max-depths of recursive algorithm
  %   E  #V list of total evaluations of recursive algorithm
  %

  function I = inpolygon_convex(V,C);
    %I = inpolygon(V(:,1),V(:,2),C(:,1),C(:,2));
    %I = abs(winding_number(C,[1:size(C,1);2:size(C,1) 1]',V))>0.5;
    %% Geez, this is barely faster than winding_number
    %Nx = C([2:end 1],2)-C(:,2);
    %Ny = C(:,1)-C([2:end 1],1);
    %S = (V(:,1)-C(:,1)').*Nx' + (V(:,2)-C(:,2)').*Ny';
    %I = all(S<0,2);
    % Oh, matlab, why:
    I = all(((V(:,1)-C(:,1)').*(C([2:end 1],2)-C(:,2))' + (V(:,2)-C(:,2)').*(C(:,1)-C([2:end 1],1))')<0,2);
  end

  if nargin<3 || isempty(ispoly)
    ispoly = false;
  end

  max_depth = 40;
  W = zeros(size(V,1),1);
  D = zeros(size(V,1),1);
  E = zeros(size(V,1),1);
  k = 0;
  % "queue"
  Q = {{C,(1:size(V,1))',1}};
  while ~isempty(Q)
    % Pop off back: faster but uglier traversal for animation
    Qi = Q{end};
    Ci = Qi{1};
    Ji = Qi{2};
    depth = Qi{3};
    Q = Q(1:end-1);
    %% Pop off front: better looking traversal for animation, but slower
    %Qi = Q{1};
    %Ci = Qi{1};
    %Ji = Qi{2};
    %depth = Qi{3};
    %Q = Q(2:end);
    D(Ji) = max(D(Ji),depth);
    E(Ji) = E(Ji)+1;
    if depth >= max_depth
      %warning('spline_winding_number exceeded max depth (%d)\n',max_depth);
      continue;
    end
    if size(Ci,1)<3
      I = false(numel(Ji),1);
    else
      H = convhull(Ci);
      I = inpolygon_convex(V(Ji,:),Ci(H(1:end-1),:));
    end
    Wi = winding_number(Ci,[size(Ci,1) 1],V(Ji(~I),:));
    W(Ji(~I)) = W(Ji(~I)) - Wi;
    %WW = zeros(size(W));
    %WW(Ji(~I)) = -Wi;
    %ss = [915 938];
    %surf( ...
    %  reshape(V(:,1),ss), ...
    %  reshape(V(:,2),ss), ...
    %  reshape(0*V(:,2),ss), ...
    %  'CData', ...
    %  reshape(WW,ss), fphong, 'EdgeColor', 'none');
    %hold on
    %[pe,p] = plot_cubic(C);
    %set(p,'Color',0.75*[1 1 1]);
    %arrayfun(@(p) set(p,'Color',1+0.5*(get(p,'Color')-1)),pe);
    %plot_cubic(Ci);
    %plot_edges(Ci,[1 4],':','LineWidth',2,'Color',0.4*[1 1 1]);
    %hold off;
    %view(2);
    %axis equal;
    %axis tight;
    %caxis([-1 1])
    %colormap((flipud(cbrewer('RdBu',45))))
    %set(gca,'YDir','reverse');
    %set(gcf,'color','w');
    %set(gca,'Visible','off','Position',[0 0 1 1]);
    %figpng(sprintf('cubic-winding_number-%02d.png',k));
    %k=k+1;

    Ki = Ji(I);
    if ~isempty(Ki)
      if ispoly
        s = ceil(size(Ci,1)/2);
        C1 = Ci(1:s,:);
        C2 = Ci(s:end,:);
        Q{end+1} = {C1,Ki,Qi{3}+1};
        Q{end+1} = {C2,Ki,Qi{3}+1};
      else
        [C1,C2] = cubic_split(Ci,0.5);
        Q{end+1} = {C1,Ki,Qi{3}+1};
        Q{end+1} = {C2,Ki,Qi{3}+1};
      end
    end
  end
end


function a=ighmap(b)
%IGHMAP Single-level inverse discrete 2-D multiwavelet transform.
%   IGHMAP performs a single-level 2-D multiwavelet reconstruction
%   using GHM multiwavelet with four multi-filters
%
%   Y = IGHMAP(X) computes the original matrix from the approximation
%   coefficients matrix LL and details coefficients matrices
%   LH, HL, HH, obtained by a multiwavelet decomposition of the
%   original input matrix and puts the result in Y.
%
%   The size of Y is the same as that of X which should be
%   a square matrix of size NxN where N is power of 2 since
%   X is de-vectorized by critical sampling preprocessing.
%   The postprocessing filter is of order 2 degree 1.
%   LL, LH, HL, and HH should have the size N/2xN/2.
%
%   X should be arranged as [LL,LH;HL,HH].
%
%   See also GHM, IGHM, GHMAP, GHMAP2, IGHMAP2, WAVEDEC2, WAVEINFO.

%   Auth: Dr. Bessam Z. Hassan
%   Last Revision: 27-Feb-2004.
%   Copyright 1995-2002 The MathWorks, Inc.
% $Revision: 1.0 $

if nargin == 0,
	error('Not enough input arguments.');
end
if isempty(b)
   a = [];
   return
end
H0=[3/(5*sqrt(2)),4/5;-1/20,-3/(10*sqrt(2))];
H1=[3/(5*sqrt(2)),0;9/20,1/sqrt(2)];
H2=[0,0;9/20,-3/(10*sqrt(2))];
H3=[0,0;-1/20,0];
G0=[-1/20,-3/(10*sqrt(2));1/(10*sqrt(2)),3/10];
G1=[9/20,-1/sqrt(2);-9/(10*sqrt(2)),0];
G2=[9/20,-3/(10*sqrt(2));9/(10*sqrt(2)),-3/10];
G3=[-1/20,0;-1/(10*sqrt(2)),0];
[N,M]=size(b);
if N~=2^round(log(N)/log(2))
    error('size of the input should be power of 2');
end
if M~=N
    error('the input matrix must be square');
end
w=[H0,H1,H2,H3;G0,G1,G2,G3];
for i=1:N/4-1
    W(4*(i-1)+1:4*i,4*i-3:4*i+4)=w;
end
W=[W;[[H2,H3;G2,G3],zeros(4,N-8),[H0,H1;G0,G1]]];
p=[];X=[];

% shuffle

N=N/2;
b1=b(1:N,1:N);b2=b(1:N,N+1:2*N);b3=b(N+1:2*N,1:N);b4=b(N+1:2*N,N+1:2*N);
b1=b1';b2=b2';b3=b3';b4=b4';
T(1:2:N,:)=b1(1:N/2,:);T(2:2:N,:)=b1(N/2+1:N,:);T=T';b1(1:2:N,:)=T(1:N/2,:);b1(2:2:N,:)=T(N/2+1:N,:);
T(1:2:N,:)=b2(1:N/2,:);T(2:2:N,:)=b2(N/2+1:N,:);T=T';b2(1:2:N,:)=T(1:N/2,:);b2(2:2:N,:)=T(N/2+1:N,:);
T(1:2:N,:)=b3(1:N/2,:);T(2:2:N,:)=b3(N/2+1:N,:);T=T';b3(1:2:N,:)=T(1:N/2,:);b3(2:2:N,:)=T(N/2+1:N,:);
T(1:2:N,:)=b4(1:N/2,:);T(2:2:N,:)=b4(N/2+1:N,:);T=T';b4(1:2:N,:)=T(1:N/2,:);b4(2:2:N,:)=T(N/2+1:N,:);
b=[b1,b2;b3,b4];
p=b';

%column vector permutation

ii=0:4:2*N-1;jj=sort([ii+1,ii+2]);kk=sort([ii+3,ii+4]);
z(jj,:)=p(1:N,:);z(kk,:)=p(N+1:2*N,:);
N=N*2;

%inverse column transform

X=W'*z;

%postprocess columns

aa(2:2:N,:)=X(2:2:N,:)/(sqrt(2)-1);
aa(1:2:N,:)=(X(1:2:N,:)-0.11086198019724*aa(2:2:N,:)-0.11086198019724*[zeros(1,N);aa(2:2:N-1,:)])/0.37361535735427;
p=aa';N=N/2;

%row vector permutation

ii=0:4:2*N-1;jj=sort([ii+1,ii+2]);kk=sort([ii+3,ii+4]);z=[];
z(jj,:)=p(1:N,:);z(kk,:)=p(N+1:2*N,:);

%inverse row transform

X=W'*z;N=N*2;

%postprocess rows

a(2:2:N,:)=X(2:2:N,:)/(sqrt(2)-1);
a(1:2:N,:)=(X(1:2:N,:)-0.11086198019724*a(2:2:N,:)-0.11086198019724*[zeros(1,N);a(2:2:N-1,:)])/0.37361535735427;


function signal = toneBurst(sample_freq, signal_freq, num_cycles, varargin)
%TONEBURST Create an enveloped single frequency tone burst.
%
% DESCRIPTION:
%       toneBurst creates an enveloped single frequency tone burst for use
%       in ultrasound simulations. If an array is given for the optional
%       input 'SignalOffset', a matrix of tone bursts is created where each
%       row corresponds to a tone burst for particular value of the
%       'SignalOffset'. If a value for the optional input 'SignalLength' is
%       given, the tone burst/s are zero padded to this length (in
%       samples).
%
% USAGE:
%       signal = toneBurst(sample_freq, signal_freq, num_cycles)
%       signal = toneBurst(sample_freq, signal_freq, num_cycles, ...)
%
% INPUTS:
%       sample_freq     - sampling frequency [Hz]
%       signal_freq     - frequency of the tone burst signal [Hz]
%       num_cycles      - number of sinusoidal oscillations
%
% OPTIONAL INPUTS:
%       Optional 'string', value pairs that may be used to modify the
%       default computational settings.
%
%       'Envelope'      - envelope used to taper the tone burst, can be set to
%                         either 'Gaussian' (default) or 'Rectangular'
%       'Plot'          - Boolean controlling whether the created tone
%                         burst is plotted
%       'SignalLength'  - signal length in number of samples, if longer
%                         than the tone burst length, the signal is
%                         appended with zeros.
%       'SignalOffset'  - signal offset before the tone burst starts in
%                         number of samples
%
% OUTPUTS:
%       signal      - created tone burst
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 4th December 2009
%       last update - 21st December 2011
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox
%
% See also gaussian

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave 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 Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% set usage defaults
num_req_input_variables = 3;
envelope = 'Gaussian';
signal_length = [];
signal_offset = 0;
plot_signal = false;

% replace with user defined values if provided
if nargin < num_req_input_variables
    error('Incorrect number of inputs');
elseif ~isempty(varargin)
    for input_index = 1:2:length(varargin)
        switch varargin{input_index}
            case 'Envelope'
                envelope = varargin{input_index + 1};
            case 'Plot'
                plot_signal = varargin{input_index + 1};
            case 'SignalOffset'
                signal_offset = varargin{input_index + 1};
                signal_offset = round(signal_offset);       % force integer
            case 'SignalLength'
                signal_length = varargin{input_index + 1};
                signal_length = round(signal_length);       % force integer
            otherwise
                error('Unknown optional input');
        end
    end
end

% calculate the temporal spacing
dt = 1/sample_freq;

% create the tone burst
tone_length = num_cycles/(signal_freq);
tone_t = 0:dt:tone_length;
tone_burst = sin(2*pi*signal_freq*tone_t);
tone_index = round(signal_offset);

% create the envelope
switch envelope
    case 'Gaussian'
        x_lim = 3;
        window_x = -x_lim:2*x_lim/(length(tone_burst)-1):x_lim;
        window = gaussian(window_x, 1, 0, 1);
    case 'Rectangular'
        window = ones(size(tone_burst));
    otherwise
        error(['Unknown envelope ' envelope]);
end

% apply the envelope
tone_burst = tone_burst.*window;

% force the ends to be zero by applying a second window
tone_burst = tone_burst.*(getWin(length(tone_burst), 'Tukey', 'Param', 0.05).');

% % calculate the expected FWHM in the frequency domain
% t_var = tone_length/(2*x_lim);
% w_var = 1/(4*pi^2*t_var);
% fw = 2 * sqrt(2 * log(2) * w_var)

% create the signal with the offset tone burst
if isempty(signal_length)
    signal = zeros(length(tone_index), max(signal_offset(:)) + length(tone_burst));
else
    signal = zeros(length(tone_index), signal_length);
end
for offset = 1:length(tone_index)
    signal(offset, tone_index(offset) + 1:tone_index(offset) + length(tone_burst)) = tone_burst;
end

% plot the signal if required
if plot_signal

    % compute suitable axis scaling
    time_axis = (0:length(signal)-1)*dt;
    [t_sc, scale, prefix] = scaleSI(max(time_axis(:)));

    % create figure
    figure;
    if numel(signal_offset) == 1
        plot(time_axis*scale, signal, 'k-');
    else
        plot(time_axis*scale, signal + repmat(2*max(signal(:))*(1:length(signal_offset)).', 1, length(time_axis)), 'k-');
    end
    xlabel(['Time [' prefix 's]']);
    ylabel('Signal Amplitude');
    axis tight;

end


function HTotal=spherAngUvCrossHessian(uv,systemType,Ms,Muv)
%%SPHERANGUVCROSSHESSIAN Determine second partial derivative matrices of 3D
%               spherical angular components with respect to u-v direction
%               cosines.
%
%INPUTS: uv A 2XN or 3XN (if the third components of the unit vector) set
%          of direction [u;v;w] cosines values in 3D. If the third
%          component of the unit vector is omitted, it is assumed to be
%          positive.
% systemType An optional parameter specifying the axes from which the
%          angles for the spherical coordinate system are measured in
%          radians. Possible vaues are
%          0 (The default if omitted) Azimuth is measured counterclockwise
%            from the x-axis in the x-y plane. Elevation is measured up
%            from the x-y plane (towards the z-axis). This is consistent
%            with common spherical coordinate systems for specifying
%            longitude (azimuth) and geocentric latitude (elevation).
%          1 Azimuth is measured counterclockwise from the z-axis in the
%            z-x plane. Elevation is measured up from the z-x plane
%            (towards the y-axis). This is consistent with some spherical
%            coordinate systems that use the z axis as the boresight
%            direction of the radar.
%          2 This is the same as 0 except instead of being given
%            elevation, one desires the angle away from the z-axis, which
%            is (pi/2-elevation).
%   Ms,Muv If either the spherical coordinate system or the u-v coordinate
%          system is rotated compared to the global Cartesian coordinate
%          system, these optional 3X3 matrices provide the rotations. Ms
%          is a 3X3 matrix to go from the alignment of a global
%          Cartesian coordinate system to that in which the spherical
%          coordinates are computed. Similarly, Muv is a rotation matrix
%          to go from the alignment of a global Cartesian cordinate system
%          to that in which the u-v(-w) coordinates are computed. If
%          either of these in omitted or an empty matrix is passed, then
%          the missing one is replaced with the identity matrix.
%
%OUTPUTS: HTotal A 2X2X2XN matrix of second derivatives where
%                HTotal(:,:,i,j) is the Hessian matrix of the ith
%                component of [azimuth;elevation] evaluated at the jth
%                point in uv. The ordering of the second derivatives in the
%                i,jth Hessian matrix is [d/du^2, d/(dudv);
%                                         d/(dudv), d/dv^2]
%
%EXAMPLE:
%Here, we verify that the Hessian matrix computed by this function is close
%to the computed using forward differencing of the gradient.
% systemType=0;
% uv=[0.1;-0.2];
%
% H=spherAngUvCrossHessian(uv,systemType);
% J=spherAngUvCrossGrad(uv,systemType);
% epsVal=1e-8;
% uv1=uv+[epsVal;0];
% J1=spherAngUvCrossGrad(uv1,systemType);
% dAz=(J1-J)/epsVal;
%
% uv1=uv+[0;epsVal];
% J1=spherAngUvCrossGrad(uv1,systemType);
% dEl=(J1-J)/epsVal;
%
% HNumDiff=zeros(2,2,2,1);
%
% %Derivatives of azimuth
% HNumDiff(1,1,1)=dAz(1,1);%dAz/du^2
% HNumDiff(1,2,1)=dAz(1,2);%dAz/(dudv)
% HNumDiff(2,1,1)=HNumDiff(1,2,1);
% HNumDiff(2,2,1)=dEl(1,2);%dAz/dv^2
%
% %Derivatives of elevation
% HNumDiff(1,1,2)=dAz(2,1);%dEl/du^2
% HNumDiff(1,2,2)=dAz(2,2);%dEl/(dudv)
% HNumDiff(2,1,2)=HNumDiff(1,2,2);
% HNumDiff(2,2,2)=dEl(2,2);%dEl/dv^2
%
% max(abs(HNumDiff(:)-H(:)))
%One will see that the difference between the true Hessian and the Hessian
%from numeric differentiation is on the order of 8e-7, which indicates good
%agreement.
%
%June 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(Muv))
    Muv=eye(3,3);
end

if(nargin<3||isempty(Ms))
    Ms=eye(3,3);
end

if(nargin<2||isempty(systemType))
   systemType=0;
end

hasW=size(uv,1)>2;
N=size(uv,2);

M=Ms*Muv';
m11=M(1,1);
m12=M(1,2);
m13=M(1,3);
m21=M(2,1);
m22=M(2,2);
m23=M(2,3);
m31=M(3,1);
m32=M(3,2);
m33=M(3,3);

HTotal=zeros(2,2,2,N);
for curPoint=1:N
    if(hasW==false)
        uvwCur=[uv(:,curPoint);sqrt(1-uv(1,curPoint)^2-uv(2,curPoint)^2)];
    else
        uvwCur=uv(:,curPoint);
    end
    u=uvwCur(1);
    v=uvwCur(2);
    w=uvwCur(3);

    uvwCur=M*uvwCur;
    u1=uvwCur(1);
    v1=uvwCur(2);
    w1=uvwCur(3);

    %First derivatives
    du1du=m11-m13*u/w;
    du1dv=m12-m13*v/w;
    dv1du=m21-m23*u/w;
    dv1dv=m22-m23*v/w;
    dw1du=m31-m33*u/w;
    dw1dv=m32-m33*v/w;

    w3=w^3;
    %Second derivatives
    d2u1dudu=m13*(v^2-1)/w3;
    d2u1dudv=-m13*u*v/w3;
    d2u1dvdv=m13*(u^2-1)/w3;
    d2v1dudu=m23*(v^2-1)/w3;
    d2v1dudv=-m23*u*v/w3;
    d2v1dvdv=m23*(u^2-1)/w3;
    d2w1dudu=m33*(v^2-1)/w3;
    d2w1dudv=-m33*u*v/w3;
    d2w1dvdv=m33*(u^2-1)/w3;

    switch(systemType)
        case 0
            denom1=(u1^2+v1^2)^2;
            denom2=sqrt(1-w1^2)^3;

            dazdu2=(-2*u1*(2*du1du*dv1du*u1-du1du^2*v1+dv1du^2*v1)+(2*du1du*dv1du+d2v1dudu*u1-d2u1dudu*v1)*(u1^2+v1^2))/denom1;
            dazdudv=(d2v1dudv*u1^3+v1^2*(du1dv*dv1du+du1du*dv1dv-d2u1dudv*v1)-u1^2*(du1dv*dv1du+du1du*dv1dv+d2u1dudv*v1)+u1*v1*(2*du1du*du1dv-2*dv1du*dv1dv+d2v1dudv*v1))/denom1;
            dazdv2=(-2*u1*(2*du1dv*dv1dv*u1-du1dv^2*v1+dv1dv^2*v1)+(2*du1dv*dv1dv+d2v1dvdv*u1-d2u1dvdv*v1)*(u1^2+v1^2))/denom1;
            deldu2=(d2w1dudu+dw1du^2*w1-d2w1dudu*w1^2)/denom2;
            deldudv=(d2w1dudv+dw1du*dw1dv*w1-d2w1dudv*w1^2)/denom2;
            deldv2=(d2w1dvdv+dw1dv^2*w1-d2w1dvdv*w1^2)/denom2;

%If there were no rotations, it would be:
%             u2v2=u^2+v^2;
%             dazdu2=(2*u*v)/u2v2^2;
%             dazdudv=(-u^2+v^2)/u2v2^2;
%             dazdv2=-((2*u*v)/u2v2^2);
%             deldu2=-((u^4+v^2-v^4)/(w*sqrt(u2v2))^3);
%             deldudv=-((u*v*(-1+2*u^2+2*v^2))/(w*sqrt(u2v2))^3);
%             deldv2=-((u^2-u^4+v^4)/(w*sqrt(u2v2))^3);
        case 1
            denom1=(u1^2+w1^2)^2;
            denom2=sqrt(1-v1^2)^3;

            dazdu2=(2*u1*(2*du1du*dw1du*u1-du1du^2*w1+dw1du^2*w1)+(-2*du1du*dw1du-d2w1dudu*u1+d2u1dudu*w1)*(u1^2+w1^2))/denom1;
            dazdudv=(u1^2*(du1dv*dw1du+du1du*dw1dv-d2w1dudv*u1)+u1*(-2*du1du*du1dv+2*dw1du*dw1dv+d2u1dudv*u1)*w1-(du1dv*dw1du+du1du*dw1dv+d2w1dudv*u1)*w1^2+d2u1dudv*w1^3)/denom1;
            dazdv2=(2*u1*(2*du1dv*dw1dv*u1-du1dv^2*w1+dw1dv^2*w1)+(-2*du1dv*dw1dv-d2w1dvdv*u1+d2u1dvdv*w1)*(u1^2+w1^2))/denom1;
            deldu2=(d2v1dudu+dv1du^2*v1-d2v1dudu*v1^2)/denom2;
            deldudv=(d2v1dudv+dv1du*dv1dv*v1-d2v1dudv*v1^2)/denom2;
            deldv2=(d2v1dvdv+dv1dv^2*v1-d2v1dvdv*v1^2)/denom2;

%If there were no rotations, it would be:
%             dazdu2=u/w^3;
%             dazdudv=v/w^3;
%             dazdv2=-((u*(-1+u^2+(-1+u^2)*v^2+2*v^4))/(w^3*(-1+v^2)^2));
%             deldu2=0;
%             deldudv=0;
%             deldv2=v/(1-v^2)^(3/2);
        case 2
            denom1=(u1^2+v1^2)^2;
            denom2=sqrt(1-w1^2)^3;

            dazdu2=(-2*u1*(2*du1du*dv1du*u1-du1du^2*v1+dv1du^2*v1)+(2*du1du*dv1du+d2v1dudu*u1-d2u1dudu*v1)*(u1^2+v1^2))/denom1;
            dazdudv=(d2v1dudv*u1^3+v1^2*(du1dv*dv1du+du1du*dv1dv-d2u1dudv*v1)-u1^2*(du1dv*dv1du+du1du*dv1dv+d2u1dudv*v1)+u1*v1*(2*du1du*du1dv-2*dv1du*dv1dv+d2v1dudv*v1))/denom1;
            dazdv2=(-2*u1*(2*du1dv*dv1dv*u1-du1dv^2*v1+dv1dv^2*v1)+(2*du1dv*dv1dv+d2v1dvdv*u1-d2u1dvdv*v1)*(u1^2+v1^2))/denom1;
            deldu2=(-dw1du^2*w1+d2w1dudu*(-1+w1^2))/denom2;
            deldudv=(-dw1du*dw1dv*w1+d2w1dudv*(-1+w1^2))/denom2;
            deldv2=(-dw1dv^2*w1+d2w1dvdv*(-1+w1^2))/denom2;

%If there were no rotations, it would be:
%             u2v2=u^2+v^2;
%             dazdu2=(2*u*v)/u2v2^2;
%             dazdudv=(-u^2+v^2)/u2v2^2;
%             dazdv2=-((2*u*v)/u2v2^2);
%             deldu2=(u^4+v^2-v^4)/(w*sqrt(u2v2))^3;
%             deldudv=(u*v*(-1+2*u^2+2*v^2))/(w*sqrt(u2v2))^3;
%             deldv2=(u^2-u^4+v^4)/(w*sqrt(u2v2))^3;
        case 3
            denom1=(u1^2+v1^2)^2;
            denom2=sqrt(1-w1^2)^3;

            dazdu2=(2*u1*(2*du1du*dv1du*u1-du1du^2*v1+dv1du^2*v1)+(-2*du1du*dv1du-d2v1dudu*u1+d2u1dudu*v1)*(u1^2+v1^2))/denom1;
            dazdudv=(u1^2*(du1dv*dv1du+du1du*dv1dv-d2v1dudv*u1)+u1*(-2*du1du*du1dv+2*dv1du*dv1dv+d2u1dudv*u1)*v1-(du1dv*dv1du+du1du*dv1dv+d2v1dudv*u1)*v1^2+d2u1dudv*v1^3)/denom1;
            dazdv2=(2*u1*(2*du1dv*dv1dv*u1-du1dv^2*v1+dv1dv^2*v1)+(-2*du1dv*dv1dv-d2v1dvdv*u1+d2u1dvdv*v1)*(u1^2+v1^2))/denom1;
            deldu2=(d2w1dudu+dw1du^2*w1-d2w1dudu*w1^2)/denom2;
            deldudv=(d2w1dudv+dw1du*dw1dv*w1-d2w1dudv*w1^2)/denom2;
            deldv2=(d2w1dvdv+dw1dv^2*w1-d2w1dvdv*w1^2)/denom2;

%If there were no rotations, it would be:
%             u2v2=u^2+v^2;
%             dazdu2=-(2*u*v)/u2v2^2;
%             dazdudv=(u-v)*(u+v)/u2v2^2;
%             dazdv2=(2*u*v)/u2v2^2;
%             deldu2=-((u^4+v^2-v^4)/(w*sqrt(u2v2))^3);
%             deldudv=-((u*v*(-1+2*u^2+2*v^2))/(w*sqrt(u2v2))^3);
%             deldv2=-((u^2-u^4+v^4)/(w*sqrt(u2v2))^3);
        otherwise
            error('Invalid system type specified.')
    end

    H=zeros(2,2,2);
    %Derivatives of azimuth
    H(1,1,1)=dazdu2;
    H(1,2,1)=dazdudv;
    H(2,1,1)=H(1,2,1);
    H(2,2,1)=dazdv2;
    %Derivatives of elevation
    H(1,1,2)=deldu2;
    H(1,2,2)=deldudv;
    H(2,1,2)=H(1,2,2);
    H(2,2,2)=deldv2;

    HTotal(:,:,:,curPoint)=H;
end
end

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


function vr = rotateVector(v, angle)
%ROTATEVECTOR Rotate a vector by a given angle.
%
%   VR = rotateVector(V, THETA)
%   Rotate the vector V by an angle THETA, given in radians.
%
%   Example
%   rotateVector([1 0], pi/2)
%   ans =
%       0   1
%
%   See also
%   vectors2d, transformVector, createRotation

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2011-04-14, using Matlab 7.9.0.529 (R2009b)
% Copyright 2011-2022 INRA - Cepia Software Platform

% precomputes angles
cot = cos(angle);
sit = sin(angle);

% compute rotated coordinates
vr = [cot * v(:,1) - sit * v(:,2) , sit * v(:,1) + cot * v(:,2)];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve the following exercises:
%   A) Exercise A: Use the Runge-Kutta 4 algorithm to integrate the
%   differential equation:
%            dy/dt = 2*t
%      Integrate from t0=0, to tfinal = 10 s.
%      Compare the solution with the algebraic integration.
%   B) Exercise B: Use the Runge-Kutta 4 algorithm to integrate the
%   second order differential equation:
%            d2y/dt^2+5*dy/dt + 4*y(t) = 0
%
%   C) Exercise C: Use the Runge-Kutta 4 algorithm to simulate the movement
%   of a 1 dof robot arm with friction under the effect of gravity and with
%   zero torque applied.
%
%   D) Exercise D: Use the Runge-Kutta 4 algorithm to simulate the movement
%   of a 2 dof robot arm with friction under the effect of gravity and with
%   zero torques applied.
%
% Help: Function prototype
% [y, t] = runge_kutta(f, y0, [t0 tfinal], timestep)
% where f is the function being integrated as dy/dt = f(t, y).
% y0 are the initial conditions
% t0: initial time
% tfinal: final time.
% h: time step for the calculations.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function runge_kutta_exercises()
close all;

%uncomment to execute each of the exercises
%exerciseA()
%exerciseB()
%exerciseC()
exerciseD()

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Integrate a simple time function. dy/dt = 2*t
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exerciseA()
t0 = 0;
tfinal = 10;
% TODO: define the line function below.
[t, y] = runge_kutta(@line, 0, [t0 tfinal], 0.1);

%Compare with the integral of 2*t
%error = y-t.^2';
%mean(error)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use Runge-Kutta to integrate a second order equation of the form.
% d2y/dt^2+5*dy/dt + 4*y(t) = 0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exerciseB()
t0 = 0;
tfinal = 10;
[t, y] = runge_kutta(@second_order_system, [10 1], [t0 tfinal], 0.1);

%plot results
plot(t, y)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now use Runge-kutta to integrate the movement of a 1 dof robot arm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exerciseC()
%these variables are shared by the forward_dynamic_robot1 function defined
%below
global robot tau g
t0 = 0;
tfinal = 10;
robot = load_robot('example','1dofplanar')
robot.dynamics.friction=0
tau = [0];
g = [0 -9.81 0]';
[t, y] = runge_kutta(@forward_dynamic_robot1, [0 0]', [t0 tfinal], 0.01);

% Animate the movement. Change speed from 1-30-100-200
speed = 10
animate(robot,[y(1,1:speed:length(y))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now use Runge-Kutta to simulate the movement of a 2 DOF robot arm.
function exerciseD()
global robot tau g
t0 = 0;
tfinal = 10;
robot = load_robot('example','2dofplanar')
robot.dynamics.friction=1
tau = [0 0];
g = [0 -9.81 0]';
[t, y] = runge_kutta(@forward_dynamic_robot2, [0 0 0 0]', [t0 tfinal], 0.001);
%speed, change to 10, 30, 50, 100
speed = 50
animate(robot,[y(1,1:speed:length(y)); y(2,1:speed:length(y))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function.
% The function returns dy/dt = 2*t. Function called from exerciseA()
%
% Integrate a line in time. dy/dt = 2*t. Obviously, the integration should
% yield. y(t) = t^2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function dy = line(t, y)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to solve a second order differential equation.
% Called from function exerciseB()
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = second_order_system(t, y)

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to simulate the movement of a 1 DOF robot
% Called from function exerciseC()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = forward_dynamic_robot1(t, y)
global tau g robot

qdd = accel(robot, y(1), y(2), tau, g);
%return qd, qdd
xd = [y(2); qdd];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to simulate the movement of a 2 DOF robot
% Called from function exerciseD()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = forward_dynamic_robot2(t, y)
global tau g robot
%we must return the solution of
% [dx1/dt; dx2/dt]
t
qdd=forwarddynamics_2dofplanar(robot, y(1:2,1), y(3:4,1), tau', 9.81, [0 0 0 0 0 0]);
%return qd, qdd
xd = [y(3:4,1); qdd];


% [INPUT]
% data = A float t-by-2 matrix (-Inf,Inf) representing the model input.
% e = A float (0,2] representing the exponent of the euclidean distance used to calculate the Distance Correlation (optional, default=1).
%
% [OUTPUT]
% dcor = A float [0,1] representing the Distance Correlation.
% rmss = A float [0,1] representing the RMS Similarity.

function [dcor,rmss] = similarity_statistics(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('data',@(x)validateattributes(x,{'double'},{'real' 'finite' '2d' 'nonempty' 'size' [NaN 2]}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    data = validate_input(ipr.data);

    nargoutchk(2,2);

    [dcor,rmss] = similarity_statistics_internal(data);

end

function [dcor,rmss] = similarity_statistics_internal(data)

    x1 = data(:,1);
    x2 = data(:,2);

    dcor = calculate_dcor(x1,x2);
    rmss = calculate_rmss(x1,x2);

end

function dcor = calculate_dcor(x1,x2)

    d1 = sqrt(bsxfun(@minus,x1,x1.').^2);
    m1 = mean(d1,1);
    k1 = bsxfun(@minus,bsxfun(@minus,d1,m1.'),m1) + mean(mean(d1));

    d2 = sqrt(bsxfun(@minus,x2,x2.').^2);
    m2 = mean(d2,1);
    k2 = bsxfun(@minus,bsxfun(@minus,d2,m2.'),m2) + mean(mean(d2));

    v1 = sqrt(mean(mean(k1 .* k1)));
    v2 = sqrt(mean(mean(k2 .* k2)));
    v = sqrt(v1 * v2);

    dcov = sqrt(mean(mean(k1 .* k2)));

    if (v > 0)
        dcor = dcov / v;
    else
        dcor = 0;
    end

end

function rmss = calculate_rmss(x1,x2)

    s = 1 - (abs(x1 - x2) ./ (abs(x1) + abs(x2)));
    rmss = sqrt(mean(s.^2,'omitnan'));

end

function data = validate_input(data)

    t = size(data,1);

    if (t < 5)
        error('The value of ''data'' is invalid. Expected input to be a matrix with at least 5 rows.');
    end

end


function indx = r8vec_indexed_heap_d ( n, a, indx )

%*****************************************************************************80
%
%% R8VEC_INDEXED_HEAP_D creates a descending heap from an indexed R8VEC.
%
%  Discussion:
%
%    An R8VEC is a vector of R8's.
%
%    An indexed R8VEC is an R8VEC of data values, and an R8VEC of N indices,
%    each referencing an entry of the data vector.
%
%    The function adjusts the index vector INDX so that, for 1 <= J <= N/2,
%    we have:
%      A(INDX(2*J))   <= A(INDX(J))
%    and
%      A(INDX(2*J+1)) <= A(INDX(J))
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Albert Nijenhuis, Herbert Wilf,
%    Combinatorial Algorithms for Computers and Calculators,
%    Academic Press, 1978,
%    ISBN: 0-12-519260-6,
%    LC: QA164.N54.
%
%  Parameters:
%
%    Input, integer N, the size of the index array.
%
%    Input, real A(*), the data vector.
%
%    Input, integer INDX(N), the index array.
%    Each entry of INDX must be a valid index for the array A.
%
%    Output, integer INDX(N), the indices have been reordered into a
%    descending heap.
%

%
%  Only nodes N/2 down to 1 can be "parent" nodes.
%
  for i = floor ( n / 2 ) : -1 : 1
%
%  Copy the value out of the parent node.
%  Position IFREE is now "open".
%
    key = indx(i);
    ifree = i;

    while ( 1 )
%
%  Positions 2*IFREE and 2*IFREE + 1 are the descendants of position
%  IFREE.  (One or both may not exist because they exceed N.)
%
      m = 2 * ifree;
%
%  Does the first position exist?
%
      if ( n < m )
        break
      end
%
%  Does the second position exist?
%
      if ( m + 1 <= n )
%
%  If both positions exist, take the larger of the two values,
%  and update M if necessary.
%
        if ( a(indx(m)) < a(indx(m+1)) )
          m = m + 1;
        end

      end
%
%  If the large descendant is larger than KEY, move it up,
%  and update IFREE, the location of the free position, and
%  consider the descendants of THIS position.
%
      if ( a(indx(m)) <= a(key) )
        break
      end

      indx(ifree) = indx(m);
      ifree = m;

    end
%
%  Once there is no more shifting to do, KEY moves into the free spot IFREE.
%
    indx(ifree) = key;

  end

  return
end


function b = subset_complement ( n, a )

%*****************************************************************************80
%
%% SUBSET_COMPLEMENT computes the complement of a set.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the order of the master set, of which A is
%    a subset.  N must be positive.
%
%    Input, integer A(N), a subset of the master set.
%    A(I) = 0 if the I-th element is in the subset A, and is
%    1 otherwise.
%
%    Output, integer B(N), the complement of A.
%

%
%  Check.
%
  subset_check ( n, a );

  b(1:n) = 1 - a(1:n);

  return
end


%{
    Tests the Dantzig Selector

    min_x ||x||_1
s.t.
    || D*A'*(A*x - b) || <= delta

The solvers solve a regularized version, using
    ||x||_1 + mu/2*||x-x_0||_2^2

see also test_sDantzig.m

This demo shows three formulations of the Dantzig selector
Instead of calling a pre-built solver, we show how to call
tfocs_SCD directly.

%}

% Before running this, please add the TFOCS base directory to your path

% Try to load the problem from disk
mu = 0;
fileName = fullfile('reference_solutions','dantzig_problem1_smoothed_noisy');
randn('state',34324);
rand('state',34324);
N = 1024;
M = round(N/2);
K = round(M/5);
A = randn(M,N);
if exist([fileName,'.mat'],'file')
    load(fileName);
    fprintf('Loaded problem from %s\n', fileName );
else
    disp('Please run test_sDantzig.m to setup the file');
end

[M,N]           = size(A);
K               = nnz(x_original);
norm_x_ref      = norm(x_ref);
norm_x_orig     = norm(x_original);
er_ref          = @(x) norm(x-x_ref)/norm_x_ref;
er_signal       = @(x) norm(x-x_original)/norm_x_orig;
resid           = @(x) norm(A*x-b)/norm(b);  % change if b is noisy

fprintf('\tA is %d x %d, original signal has %d nonzeros\n', M, N, K );
fprintf('\tl1-norm solution and original signal differ by %.2e (mu = %.2e)\n', ...
    norm(x_ref - x_original)/norm(x_original),mu );

%% Call the TFOCS solver
er              = er_ref;  % error with reference solution (from IPM)
opts = [];
opts.restart    = 500;
opts.errFcn     = { @(f,dual,primal) er(primal), ...
                    @(f,dual,primal) obj_ref - f  };
opts.maxIts     = 1000;
opts.printEvery = 100;
z0  = [];   % we don't have a good guess for the dual

%% Method 1: use the epigraph of the l_infinity norm as the cone
% Note: this is equivalent to calling:
% [ x, out, optsOut ] = solver_sDantzig( {A,D}, b, delta, mu, x0, z0, opts );

DAtb = D.*(A'*b);
DD = @(x) D.*(x);
objectiveF = prox_l1;
affineF     = {linop_matrix(diag(D)*(A'*A)), -DAtb };
dualproxF  = prox_l1( delta );
[ x, out, opts ] = tfocs_SCD( objectiveF, affineF, dualproxF, mu, x0, z0, opts );
x1 = x;
out1 = out;

fprintf('Solution has %d nonzeros.  Error vs. IPM solution is %.2e\n',...
    nnz(x), er(x) );

% Check that we are within allowable bounds
if out.err(end,1) < 1e-3
    disp('Everything is working');
else
    error('Failed the test');
end
%% Method 2: use the LP formulation
% Instead of the constraint ||Ax-b||_infty <= delta
% (where "A" is really DA'A),
% think of it as
% -(Ax-b) + delta >= 0
%  (Ax-b) + delta >= 0

objectiveF = prox_l1;
affineF     = {linop_matrix(-diag(D)*(A'*A)),  DAtb + delta;...
               linop_matrix( diag(D)*(A'*A)), -DAtb + delta; };

dualproxF  = { proj_Rplus; proj_Rplus };
[ x, out, opts ] = tfocs_SCD( objectiveF, affineF, dualproxF, mu, x0, z0, opts );
x2 = x;
out2 = out;

% Check that we are within allowable bounds
if out.err(end,1) < 1e-3
    disp('Everything is working');
else
    error('Failed the test');
end
%% Method 3: put objective into constraint
% This is the trick we do with solver_sDantzig_W, to deal with ||Wx||_1
% Instead of minimizing ||x||_1, we minimize t,
%   with the constraint that ||x||_1 <= t
% This version has some scaling considerations -- if we
% are careful about scaling, the dual problem will be solved much faster.

% Note: we still have to deal with the original constraints.  We
% can use either method 1 or method 2 above.  Here, we'll use
% method 1.

objectiveF  = [];    % tfocs_SCD recognizes this special objective
normA2      = norm( diag(D)*A'*A )^2;
scale       = 1/sqrt(normA2);
affineF     = {linop_matrix(diag(D)*(A'*A)), -DAtb; linop_scale(1/scale), 0 };
dualproxF   = { prox_l1(delta); proj_linf(scale) };
[ x, out, opts ] = tfocs_SCD( objectiveF, affineF, dualproxF, mu, x0, z0, opts );
x3 = x;
out3 = out;

% Check that we are within allowable bounds
if out.err(end,1) < 1e-2
    disp('Everything is working');
else
    error('Failed the test');
end
%% plot
figure;
semilogy( out1.err(:,1) );
hold all
semilogy( out2.err(:,1) );
semilogy( out3.err(:,1) );
legend('Method 1 (epigraph cone)','Method 2 (LP)','Method 3 (W=I)' );

%% close all plots
close all

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


%% Copyright (C) 2014, 2016, 2019 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod  @@sym laplacian (@var{f})
%% @defmethodx @@sym laplacian (@var{f}, @var{x})
%% Symbolic Laplacian of symbolic expression.
%%
%% The Laplacian of a scalar expression @var{f} is
%% the scalar expression:
%% @example
%% @group
%% syms f(x, y, z)
%% laplacian(f)
%%   @result{} (sym)
%%         2                 2                 2
%%        ∂                 ∂                 ∂
%%       ───(f(x, y, z)) + ───(f(x, y, z)) + ───(f(x, y, z))
%%         2                 2                 2
%%       ∂x                ∂y                ∂z
%% @end group
%% @end example
%%
%% @var{x} can be a scalar, vector or cell list.  If omitted,
%% it is determined using @code{symvar}.
%%
%% Example:
%% @example
%% @group
%% syms x y
%% laplacian(x^3 + 5*y^2)
%%   @result{} (sym) 6⋅x + 10
%% @end group
%% @end example
%%
%% Note: assumes @var{x} is a Cartesian coordinate system.
%%
%% @seealso{@@sym/divergence, @@sym/gradient, @@sym/curl, @@sym/jacobian,
%%          @@sym/hessian}
%% @end defmethod

function g = laplacian(f,x)

  assert (isscalar(f), 'laplacian: only scalar functions supported')

  if (nargin == 1)
    x = symvar(f);
    if (isempty(x))
      x = sym('x');
    end
  elseif (nargin == 2)
    % no-op
  else
    print_usage ();
  end

  if (~iscell(x) && isscalar(x))
    x = {x};
  end

  cmd = { '(f, x) = _ins'
          'g = 0'
          'for y in x:'
          '    g = g + f.diff(y, 2)'
          'return g,' };

  g = pycall_sympy__ (cmd, sym(f), x);

end

%!shared x,y,z
%! syms x y z

%!test
%! % 1D
%! f = x^2;
%! g = diff(f,x,x);
%! assert (isequal (laplacian(f), g))
%! assert (isequal (laplacian(f,{x}), g))
%! assert (isequal (laplacian(f,[x]), g))
%! assert (isequal (laplacian(f,x), g))

%!test
%! % const
%! f = sym(1);
%! g = sym(0);
%! assert (isequal (laplacian(f), g))
%! assert (isequal (laplacian(f,x), g))
%! f = sym('c');
%! assert (isequal (laplacian(f,x), g))

%!test
%! % double const
%! f = 1;
%! g = sym(0);
%! assert (isequal (laplacian(f,x), g))

%!test
%! % 1D fcn in 2d/3d
%! f = sin(2*y);
%! g = -4*f;
%! assert (isequal (laplacian(f), g))
%! assert (isequal (laplacian(f, {x,y}), g))
%! assert (isequal (laplacian(f, {x,y,z}), g))

%!test
%! % 2d fcn in 2d/3d
%! f = sin(exp(x)*y);
%! g = diff(f,x,x) + diff(f,y,y);
%! assert (isequal (laplacian(f), g))
%! assert (isequal (laplacian(f, {x,y}), g))

%!test
%! % 2d fcn in 2d/3d
%! f = sin(exp(x)*y+sinh(z));
%! gr2 = gradient(f, {x,y});
%! divgr2 = divergence(gr2, {x,y});
%! l2 = laplacian(f,{x,y});
%! gr3 = gradient(f, {x,y,z});
%! divgr3 = divergence(gr3, {x,y,z});
%! l3 = laplacian(f,{x,y,z});
%! assert (isAlways (l2 == divgr2))
%! assert (isAlways (l3 == divgr3))

%!error laplacian(sym('x'), sym('x'), 42)
%!error <only scalar> laplacian([sym('x'), sym('x')])


function stroud_test02 ( )

%*****************************************************************************80
%
%% TEST02 tests BALL_MONOMIAL_ND.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;

  r = 2.0;
  xc(1:n) = [ 0.0, 0.0, 0.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02\n' );
  fprintf ( 1, '  For the integral of a monomial in a ball in ND:\n' );
  fprintf ( 1, '  BALL_MONOMIAL_ND approximates the integral.\n' );
  fprintf ( 1, '  BALL_F1_ND, which can handle general integrands,\n' );
  fprintf ( 1, '    will be used for comparison.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension N = %d\n', n );
  fprintf ( 1, '  Ball radius = %f\n', r )
  fprintf ( 1, '  Ball volume = %f\n', ball_volume_nd ( n, r ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '        Rule:	   MONOMIAL	      BALL_F1_ND\n' );
  fprintf ( 1, '        F(X)\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 4

    if ( i == 1 )
      string = '1';
      p = [ 0, 0, 0 ];
      result2 = ball_f1_nd ( 'mono_000_3d', n, xc, r );
    elseif ( i == 2 )
      string = 'xyz';
      p = [ 1, 1, 1 ];
      result2 = ball_f1_nd ( 'mono_111_3d', n, xc, r );
    elseif ( i == 3 )
      string = 'x^2z^2';
      p = [ 2, 0, 2 ];
      result2 = ball_f1_nd ( 'mono_202_3d', n, xc, r );
    elseif ( i == 4 )
      string = 'x^4y^2z^2';
      p = [ 4, 2, 2 ];
      result2 = ball_f1_nd ( 'mono_422_3d', n, xc, r );
    end

    result1 = ball_monomial_nd ( n, p, r );

    fprintf ( 1, '  %10s  %14f  %14f\n', string, result1, result2 );

  end

  return
end


function [fmin,xmin]=ConjugateGradientMethod(x0)

% initialization
xk=x0;
gk=grad_obj(xk);
dk=-gk;

% iteration
for i=1:length(x0)
    % line search
    alphak=fminbnd(@(alpha) phi(alpha,xk,dk),0,10);
    % update xk
    tempgk=gk;
    tempxk=xk;
    tempdk=dk;
    xk=tempxk+alphak*tempdk;
    gk=grad_obj(xk);
    tempbetak=(gk'*(gk-tempgk))/(dk'*(gk-tempgk));
    dk=-gk+tempbetak*tempdk;
end
xmin=xk;
fmin=obj(xk);


%%% Example script showing how to perform a 3D Total-Variation filtering with proxTV

clear all
close all

% Load color image (3 dimensions: length, width and color)
X = imread('colors.png');

% Introduce noise
noiseLevel = 0.2;
N = double(imnoise(X,'gaussian',0,noiseLevel));

% Filter using 3D TV-L1: for 3D one needs to invoke prox_TVgen
lambda=100;
disp('Filtering image...');
tic;
F = prox_TVgen(N,      [lambda lambda],             [1 2],                    [1 1]);
%              Image | Penalty in each dimension |  Dimensions to penalize  | Norms to use
toc;

% Any dimension can be penalized under any norm. By also penalizing the color dimension under TV-L2 we get a "decolored" image
lambda2=5;
disp('Color filtering...');
tic;
F2 = prox_TVgen(N,      [lambda lambda lambda2],     [1 2 3],                  [1 1 2]);
%               Image | Penalty in each dimension |  Dimensions to penalize  | Norms to use
toc;

% Plot results
figure();
subplot(2,2,1);
imshow(X);
title('Original');
subplot(2,2,2);
imshow(uint8(N));
title('Noisy');
subplot(2,2,3);
imshow(uint8(F));
title('Filtered');
subplot(2,2,4);
imshow(uint8(F2));
title('Color filtered');


function[Q1,Q2,Q3,Q4,Q]=divgeom(varargin)
%DIVGEOM  Geometric decomposition of eddy vorticity flux divergence.
%
%   [F1,F2,F3,F4]=DIVGEOM(DX,DY,K,L,THETA) returns the geometric decomposition
%   of eddy vorticity flux divergence associated with variance ellipses
%   having kinetic energy K, anisotropy L, and orientation THETA.
%
%   K, L, and THETA are matrices of the same size.  These are defined on an
%   X-Y grid with x oriented in *columns* and y oriented in *rows*.  DX and
%   DY are the sampling intervals in the X and Y directions, respectively.
%
%   Note that K and L are related to ellipse parameters KAPPA and LAMBDA
%   used elsewhere in JLAB by K=KAPPA^2 and L=LAMBDA*KAPPA^2.
%
%   F1, F2, F3, and F4 are four different contributions to the eddy
%   vorticity flux divergence, as follows:
%
%       F1     Quadratic variations in the linear energy L
%       F2     Product of linear variations in THETA and L
%       F3     Quadratic variations in the orientation THETA
%       F4     Product of linear variations in orientation THETA
%
%   For details, see Waterman and Lilly (2015), Geometric decomposition of
%   eddy-mean flow feedbacks in barotropic systems, J. Phys. Oceanogr.
%
%   [F1,F2,F3,F4,F]=DIVGEOM(...) also returns the total eddy flux
%   divergence F, calculated directly, with F1+F2+F3+F4 = F apart from
%   numerical error.
%
%   By default, DIVGEOM calculates derivates with repeated applications of
%   a first central difference.  DIVGEOM(...,'arakawa') alternately uses a
%   modified first central difference appropriate for models that employ an
%   Awakawa advection scheme.
%   __________________________________________________________________
%
%   Usage: [f1,f2,f3,f4]=divgeom(dx,dy,K,L,theta);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2013--2015 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1}, '--f')
    %   'divgeom --f' generates a sample figure. XX not currently working
    %divgeom_figure,return
end

%Divgeom does run tests, but these are hidden because it involves mat-files
%not distributed as a part of JLAB
%if strcmpi(varargin{1},XXX)
%    divgeom_test,return
%end

%   Do I do this?
%   Filtering
%
%   DIVGEOM can optionally filter the second-order derivative terms, F1,
%   F3, and F to reduce small-scale noise.
%
%   [F1,F2,F3,F4,F]=DIVGEOM(...,N) smooths F1, F3, and F with an N point
%   boxcar filter in both the X and Y directions.  N should be odd.
%   __________________________________________________________________
%

diffstr='cartesian';
str='second';

for i=1:2
    if ischar(varargin{end})
        if strcmpi(varargin{end}(1:3),'car')||strcmpi(varargin{end}(1:3),'ara')
            diffstr=varargin{end};
        elseif strcmpi(varargin{end}(1:3),'dir')||strcmpi(varargin{end}(1:3),'sec')
            str=varargin{end};
        end
        varargin=varargin(1:end-1);
    end
end

dx=varargin{1};
dy=varargin{2};
K=varargin{3};
L=varargin{4};
theta=varargin{5};

if length(varargin)==6
    N=varargin{6};
else
    N=0;
end

%[x,y] = meshgrid([1:size(K,2)]*dx,[1:size(K,1)]*dx);

%/************************************************
%Compute gradients
Lx=divgeom_vdiff(dx,L,2,diffstr);
Ly=divgeom_vdiff(dy,L,1,diffstr);

thetax=divgeom_vdiff(dx,frac(1,2)*unwrap(2*theta,[],2),2,diffstr);
thetay=divgeom_vdiff(dy,frac(1,2)*unwrap(2*theta,[],1),1,diffstr);

cos2x=divgeom_vdiff(dx,cos(2*theta),2,diffstr);
cos2y=divgeom_vdiff(dy,cos(2*theta),1,diffstr);

sin2x=divgeom_vdiff(dx,sin(2*theta),2,diffstr);
sin2y=divgeom_vdiff(dy,sin(2*theta),1,diffstr);

M=L.*cos(2*theta);
N=L.*sin(2*theta);

Nx=divgeom_vdiff(dx,N,2,diffstr);
Ny=divgeom_vdiff(dy,N,1,diffstr);

QM=divgeom_mixederiv(dx,dy,divgeom_vdiff(dx,M,2,diffstr),divgeom_vdiff(dy,M,1,diffstr),diffstr);
QN=divgeom_vdiff(dx,Nx,2,diffstr)-divgeom_vdiff(dy,Ny,1,diffstr);

Q=QN-QM;
%\************************************************

%
% cx=L;
% cx=vdiff(dx,cx,2)+sqrt(-1)*vdiff(dx,cx,1);
% cx=vdiff(dx,cx,2)+sqrt(-1)*vdiff(dx,cx,1);
% Q1=-imag(rot(-2*theta).*cx);

Q1a=-cos(2*theta).*divgeom_mixederiv(dx,dy,Lx,Ly,diffstr);
Q1b= sin(2*theta).*(divgeom_vdiff(dx,Lx,2,diffstr)-divgeom_vdiff(dy,Ly,1,diffstr));
Q1=Q1a+Q1b;

%vsize(L,theta,thetax,thetay)
Q3a= 2*L.*cos(2*theta).*(divgeom_vdiff(dx,thetax,2,diffstr)-divgeom_vdiff(dy,thetay,1,diffstr));
Q3b= 2*L.*sin(2*theta).*divgeom_mixederiv(dx,dy,thetax,thetay,diffstr);
Q3=Q3a+Q3b;

if findstr(str,'dir')
    Q2a= 4*cos(2*theta).*(thetax.*Lx-thetay.*Ly);
    Q2b= 4*sin(2*theta).*(thetax.*Ly+thetay.*Lx);
    Q2=Q2a+Q2b;

    Q4a= 4*L.*cos(2*theta).*(2*thetax.*thetay);
    Q4b= -4*L.*sin(2*theta).*(thetax.^2-thetay.^2);
    Q4=Q4a+Q4b;
else
    dkdsin2=L.*(divgeom_vdiff(dx,sin2x,2,diffstr)-divgeom_vdiff(dy,sin2y,1,diffstr));
    dldcos2=L.*divgeom_mixederiv(dx,dy,divgeom_vdiff(dx,cos(2*theta),2,diffstr),...
        divgeom_vdiff(dy,cos(2*theta),1,diffstr),diffstr);

    Q2a=QN-Q1b-dkdsin2;
    Q2b=-(QM+Q1a-dldcos2);
    Q2=Q2a+Q2b;

    Q4a=-dldcos2-Q3b;
    Q4b=dkdsin2-Q3a;
    Q4=Q4a+Q4b;
end

function[df]=divgeom_vdiff(dx,f,dim,schemestr);
%This is basically just to implement what I think is the way the Arakawa
%advection scheme takes derivatives.  Taken from PSI2FIELDS

f0ca=f(:,1,:,:);
f0cb=f(:,end,:,:);
f0ra=f(1,:,:,:);
f0rb=f(end,:,:,:);

if strcmpi(schemestr(1:3),'ara')
    if dim==1
        dim2=2;
    elseif dim==2
        dim2=1;
    end
    f=frac(1,4)*(2*f + vshift(f,1,dim2) + vshift(f,-1,dim2));
    if dim==1
        %size(f),dim,size(frac(1,3)*(2*f + vshift(f,1,dim2)))
        %f(:,1,:,:)=0;
        %f(:,end,:,:)=0;
        f(:,1,:,:)=frac(1,3)*(2*f0ca + vshift(f0ca,1,dim2));
        f(:,end,:,:)=frac(1,3)*(2*f0cb + vshift(f0cb,-1,dim2));
        %f(:,end,:,:)=f0(:,end,:,:);
    elseif dim==2
        %f(1,:,:,:)=0;
        %f(end,:,:,:)=0;
        f(1,:,:,:)=frac(1,3)*(2*f0ra + vshift(f0ra,1,dim2));
        f(end,:,:,:)=frac(1,3)*(2*f0rb + vshift(f0rb,-1,dim2));
        %f(end,:,:,:)=f0(end,:,:,:);
    end
end
% dim
% size(f)
% edgestr
df=vdiff(dx,f,dim);

function[gxy]=divgeom_mixederiv(dx,dy,fx,fy,diffstr)
%Correction for two possible forms of mixed derivative

gxy= divgeom_vdiff(dy,fx,1,diffstr);
gyx= divgeom_vdiff(dx,fy,2,diffstr);
gxy=gxy+gyx;

%bool=abs(gyx)<abs(gxy);
%gxy(bool)=gyx(bool);
%gxy=2*gxy;

function[]=divgeom_test
load highresjet
use highresjet

[q1,q2,q3,q4,q]=divgeom(dx,K,L,theta);
reporttest('DIVGEOM Q1--Q4 add to total Q',aresame(q1+q2+q3+q4,q,1e-16))

[q1d,q2d,q3d,q4d,qd]=divgeom(dx,K,L,theta,'direct');

rat=abs(frac(q2-q2d,q2+q2d));
rat=sort(rat(:));
index=find(log10(rat)>-1,1,'first');

%No more than 15% of the data has the error ratio > 1/10
reporttest('DIVGEOM Q2 direct and implicit forms agree',index/length(rat)>0.85);

rat=abs(frac(q4-q4d,q4+q4d));
rat=sort(rat(:));
index=find(log10(rat)>-1,1,'first');

%No more than 15% of the data has the error ratio > 1/10
reporttest('DIVGEOM Q4 direct and implicit forms agree',index/length(rat)>0.85);

function[]=divgeom_figure
load jetellipses_highres
use jetellipses

[q1,q2,q3,q4,q]=divgeom(x(2)-x(1),x(2)-x(1),kappabar,lambdabar,thetabar,5);

figure
subplot(2,2,1),jpcolor(x,y,q1),axis equal,axis tight
subplot(2,2,2),jpcolor(x,y,q2),axis equal,axis tight
subplot(2,2,3),jpcolor(x,y,q3),axis equal,axis tight
subplot(2,2,4),jpcolor(x,y,q4),axis equal,axis tight
for i=1:4
    subplot(2,2,i),%caxis([-.2 .2]/16)
    hold on,%contour(x,y,lambdabar,[0.1 0.1]);
    %linestyle k
end
packfig(2,2)

%[q1b,q2b,q3b,q4b,qb]=divgeom(x(2)-x(1),kappabar,lambdabar,thetabar,5,'arakawa');

figure
subplot(2,1,1),jpcolor(x,y,q1+q2+q3+q4),axis equal,axis tight
subplot(2,1,2),jpcolor(x,y,q),axis equal,axis tight
for i=1:2
    subplot(2,1,i),caxis([-.2 .2]/16)
    hold on,contour(x,y,lambdabar,[0.1 0.1]);
    linestyle k
end
packfig(2,1)

function[]=divgeom_figure2

filename='/Users/lilly/Data/early/QGBetaPlaneTurbulenceFloats_experiment_04.nc';
ii=425:675;
[u,v]=vzeros(length(ii),1024,1001);
for k=1:1001
    k
    [utemp,vtemp]=FieldsFromTurbulenceFile(filename,k, 'u','v');
    u(:,:,k)=utemp(ii,:);
    v(:,:,k)=vtemp(ii,:);
end

u2=u;
v2=v;

for k=1:251
    k
    u2(k,:,:)=anatrans(u(k,:,:),3,'periodic');
    v2(k,:,:)=anatrans(v(k,:,:),3,'periodic');
end


 function xs = wls_grpr(x, G, W, yi, D, xmin, xmax, niter)
%function xs = wls_grpr(x, G, W, yi, D, xmin, xmax, niter)
%
%	weighted least squares with constraint xmin <= x <= xmax
%	using gradient projection method (polyak:87 p. 207)
%		xnew = max(xmin, xold + D * \nabla J(xold))
%	cost function: J(x) = (y-Gx) W (y-Gx) / 2
%	in
%		x	[np,1]		initial estimate
%		G	[nd,np]		system matrix
%		W	[nd,nd]		weighting matrix
%		yi	[nd,1]		noisy measurements (e.g. sinogram)
%		D	[np,np]		preconditioning / step size matrix
%		xmin			minimum allowable x value
%		xmax			maximum allowable x value
%		niter			# of iterations
%	out
%		xs	[np,niter+1]	iterates
%
%	Copyright Dec. 2000,	Jeff Fessler, University of Michigan

if nargin < 2, ir_usage, end

if ~isvar('W') || isempty(W)
	W = 1;
end
if ~isvar('niter') || isempty(niter)
	niter = 1;
end
if ~isvar('D') || isempty(D)
	D = 1;
end
if ~isvar('xmin') || isempty(xmin), xmin = 0; end
if ~isvar('xmax') || isempty(xmax), xmax = inf; end

% loop over iterations
xs = zeros(length(x), niter);
xs(:,1) = x;
for iter = 2:niter
%	printf('WLS-PG iteration %d', iter-1)

	if ~rem(iter,2)
		lgrad = G' * (W .* (yi - G * x));

		x = x + D * lgrad;	% the update!
	else

		x = max(x,xmin);	% lower bound
		x = min(x,xmax);	% upper bound
	end

	xs(:,iter) = x;
end


function [H,WtW,WtX] = nnls_fpgm(X,W,options)

% Computes an approximate solution of the following nonnegative least
% squares problem (NNLS)
%
%           min_{H >= 0} ||X-WH||_F^2
%
% using a fast gradient method;
% See Nesterov, Introductory Lectures on Convex Optimization: A Basic
% Course, Kluwer Academic Publisher, 2004.
%
% Input / Output; see nnls_input_output.m
%
% + options.proj allows to use a contraints on the columns or rows of H so
%   that the entries in each column/row sum to at most one
%   options.proj = 0: no projection (default).
%   options.proj = 1: projection of the columns on {x|x>=0, sum(x) <= 1}
%   options.proj = 2: projection of the rows {x|x>=0, sum(x) = 1}
%
% + options.alpha0 is the FPGM  extrapolation parameter (default=0.05)
%
% Code modified from https://sites.google.com/site/nicolasgillis/code
%
% This file has been ported from
%       nnls_FPGM.m at https://gitlab.com/ngillis/nmfbook/-/tree/master/algorithms
%       by Nicolas Gillis (nicolas.gillis@umons.ac.be)

    if nargin <= 2
        options = [];
    end
    if ~isfield(options,'delta')
        options.delta = 1e-6; % Stopping condition depending on evolution of the iterate V:
        % Stop if ||V^{k}-V^{k+1}||_F <= delta * ||V^{0}-V^{1}||_F
        % where V^{k} is the kth iterate.
    end
    if ~isfield(options,'inner_max_epoch')
        options.inner_max_epoch = 500;
    end
    if ~isfield(options,'proj') % Projection on the unit simplex and the origin
        options.proj = 0;
    end
    if ~isfield(options,'alpha0') % Parameter for FPGM ~ extrapolation parameter
        options.alpha0 = 0.05;
    end

    W = full(W);
    [m, n] = size(X);
    [m, r] = size(W);
    WtW = W'*W;
    WtX = W'*X;

    % If no initial matrices are provided, H is initialized as follows:
    if ~isfield(options,'init') || isempty(options.init)
        H = nnls_init(X,W,WtW,WtX);
    else
        H = options.init;
    end

    % Hessian and Lipschitz constant
    L = norm(WtW,2);
    % Linear term
    WtX = W'*X;
    alpha0 = options.alpha0; % Parameter of FPGM, can be tuned.
                             % If options.alpha0 = 0 --> no acceleration, PGM
    alpha(1) = alpha0;
    if options.proj == 1
        H = SimplexProj( H ); % Project columns of H onto the simplex and origin
    elseif options.proj == 0
        H = max(H,0);
    elseif options.proj == 2
        H = SimplexColProj(H'); % Project rows of H onto the simplex
        H = H';
    end
    Y = H; % second sequence
    i = 1;
    % Stop if ||V^{k}-V^{k+1}||_F <= delta * ||V^{0}-V^{1}||_F
    eps0 = 0; eps = 1;
    while i <= options.inner_max_epoch && eps >= options.delta*eps0
        % Previous iterate
        Hp = H;
        % FGM Coefficients; see Nesterov's book
        alpha(i+1) = ( sqrt(alpha(i)^4 + 4*alpha(i)^2 ) - alpha(i)^2) / (2);
        beta(i) = alpha(i)*(1-alpha(i))/(alpha(i)^2+alpha(i+1));
        % Projection step
        H = Y - (WtW*Y-WtX) / L;
        if options.proj == 1
            H = SimplexProj( H ); % Project columns of H onto the set {x|x>=0, sum(x) <= 1}
        elseif options.proj == 0
            H = max(H,0);
        elseif options.proj == 2
            H = SimplexColProj(H'); % Project rows of H onto the simplex
            H = H';
        end
        % `Optimal' linear combination of iterates
        Y = H + beta(i)*(H-Hp);
        if i == 1
            eps0 = norm(H-Hp,'fro');
        end
        eps = norm(H-Hp,'fro');
        i = i + 1;
    end
end


% solve the problem:
%   min_{x,e} 0.5*|z-Dx-e|_2^2 + 0.5*lambda1*|x|_2^2 + lambda2*|e|_1
%
% solve the projection by APG
% input:
%   z - data point
%   D - basis matrix
%   lambda1, lambda2 - tradeoff parameters
% output:
%   r - projection coefficient
%   e - sparse noise
% copyright Jiashi Feng (jshfeng@gmail.com)
%
function [x,e] = solve_proj2(z,D,lambda1,lambda2)
  % initialization
  [ndim,ncol] = size(D);
  e = zeros(ndim,1);
  x = zeros(ncol,1);
  I = eye(ncol);
  converged = false;
  maxIter = inf;
  iter = 0;
  % alternatively update
  DDt = inv(D'*D+lambda1*I)*D';
  while ~converged
    iter = iter + 1;
    xtemp = x;
    x = DDt*(z-e);
    %     x = (D'*D + lambda1*I)\(D'*(z-e));
    etemp = e;
    e = thres(z-D*x,lambda2);
    stopc = max(norm(e-etemp), norm(x-xtemp))/ndim;
    if stopc < 1e-6 || iter > maxIter
      converged = true;
    end
    %     fval = func_proj(z,D,lambda1,lambda2,x,e);
    %     fprintf('fval = %f\n', fval);
  end
end

function x = thres(y,mu)
  x = max(y-mu, 0);
  x = x + min(y + mu, 0);
end

function fval = func_proj(z,D,lambda1,lambda2,x,e)
  fval = 0;
  fval = fval + 0.5*norm(z-D*x-e)^2;
  fval = fval + 0.5*lambda1*norm(x)^2;
  fval = fval + lambda2*sum(abs(e));
end


function triangle_ncc_rule_test04 ( )

%*****************************************************************************80
%
%% TEST04 tests TRIANGLE_NCC_RULE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04\n' );
  fprintf ( 1, '  TRIANGLE_NCC_RULE returns the points and weights of\n' );
  fprintf ( 1, '  an NCC rule for the unit triangle.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  This routine uses those rules to estimate the\n' );
  fprintf ( 1, '  integral of monomomials in the unit triangle.\n' );

  rule_num = triangle_ncc_rule_num ( );

  area = 0.5;

  for a = 0 : 10

    for b = 0 : 10 - a
%
%  Multiplying X^A * Y^B by COEF will give us an integrand
%  whose integral is exactly 1.  This makes the error calculations easy.
%
      coef = ( a + b + 2 ) * ( a + b + 1 );
      for i = 1 : b
        coef = coef * ( a + i ) / i;
      end

      fprintf ( 1, '\n' );
      fprintf ( 1, '  Integrate %f * X^%d * Y^%d\n', coef, a, b );
      fprintf ( 1, '\n' );
      fprintf ( 1, '      Rule       QUAD           ERROR\n' );
      fprintf ( 1, '\n' );

      for rule = 1 : rule_num

        order_num = triangle_ncc_order_num ( rule );

        [ xy, w ] = triangle_ncc_rule ( rule, order_num );

        quad = 0.0;

        for order = 1 : order_num

          x = xy(1,order);
          y = xy(2,order);

          if ( a == 0 & b == 0 )
            value = coef;
          elseif ( a == 0 & b ~= 0 )
            value = coef * y^b;
          elseif ( a ~= 0 & b == 0 )
            value = coef * x^a;
          elseif ( a ~= 0 & b ~= 0 )
            value = coef * x^a * y^b;
          end

          quad = quad + w(order) * value;

        end

        quad = area * quad;

        exact = 1.0;
        err = abs ( exact - quad );

        fprintf ( 1, '  %8d  %14f  %14f\n', rule, quad, err );

      end

    end

  end

  return
end