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

%%begin

% Forward dynamics is the computation of joint accelerations given position and
% velocity state, and actuator torques.  It is useful in simulation of a robot
% control system.
%
% Consider a Puma 560 at rest in the zero angle pose, with zero applied joint
% torques. The joint acceleration would be given by

mdl_puma560
p560.accel(qz, zeros(1,6), zeros(1,6))

% To be useful for simulation this function must be integrated.  fdyn() uses the
% MATLAB function ode45() to integrate the joint acceleration.  It also allows
% for a user written function to compute the joint torque as a function of
% manipulator state.
%
% To simulate the motion of the Puma 560 from rest in the zero angle pose
% with zero applied joint torques

tic
[t q qd] = p560.nofriction().fdyn(10, [], qz);
toc

% and the resulting motion can be plotted versus time

subplot(3,1,1)
plot(t,q(:,1))
xlabel('Time (s)');
ylabel('Joint 1 (rad)')
subplot(3,1,2)
plot(t,q(:,2))
xlabel('Time (s)');
ylabel('Joint 2 (rad)')
subplot(3,1,3)
plot(t,q(:,3))
xlabel('Time (s)');
ylabel('Joint 3 (rad)')

% Clearly the robot is collapsing under gravity, but it is interesting to
% note that rotational velocity of the upper and lower arm are exerting
% centripetal and Coriolis torques on the waist joint, causing it to rotate.

% This can be shown in animation also
clf
p560.plot(q)


%% Copyright (C) 2008 Eric Chassande-Mottin
%% Copyright (C) 2011 Carnë Draug
%% Copyright (C) 2016, 2018, 2022 Colin B. Macdonald
%%
%% 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/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defun laguerreL (@var{n}, @var{x})
%% Evaluate Laguerre polynomials.
%%
%% Compute the value of the Laguerre polynomial of order @var{n}
%% for each element of @var{x}.
%% For example, the Laguerre polynomial of order 14 evaluated at
%% the point 6 is
%% @example
%% @group
%% @c doctest: +SKIP_IF(compare_versions (OCTAVE_VERSION(), '6.0.0', '<'))
%% laguerreL (14, 6)
%%   @result{} 0.9765
%% @end group
%% @end example
%%
%% This implementation uses a three-term recurrence directly on the values
%% of @var{x}.  The result is numerically stable, as opposed to evaluating
%% the polynomial using the monomial coefficients.  For example, we can
%% compare the above result to a symbolic construction:
%% @example
%% @group
%% syms x
%% L = laguerreL (14, x);
%% exact = subs (L, x, 6)
%%   @result{} exact = (sym)
%%       34213
%%       ─────
%%       35035
%% @end group
%% @end example
%% If we extract the monomial coefficients and numerically evaluate the
%% polynomial at a point, the result is rather poor:
%% @example
%% @group
%% coeffs = sym2poly (L);
%% @c doctest: +XFAIL_IF(compare_versions (OCTAVE_VERSION(), '6.0.0', '<'))
%% polyval (coeffs, 6)
%%   @result{} 0.9765
%% err = ans - double (exact);
%% num2str (err, '%.3g')
%%   @result{} -1.68e-11
%% @end group
%% @end example
%% So please don't do that!  The numerical @code{laguerreL} function
%% does much better:
%% @example
%% @group
%% err = laguerreL (14, 6) - double (exact)
%%   @result{} err = 9.9920e-16
%% @end group
%% @end example
%%
%% @seealso{@@sym/laguerreL}
%% @end defun

function L = laguerreL(n, x)

  if (nargin ~= 2)
    print_usage ();
  end

  if (any (n < 0) || any (mod (n, 1) ~= 0))
    error('second argument "n" must consist of positive integers');
  end

  if (~isscalar (n) && isscalar (x))
    x = x*ones (size (n));
  elseif (~isscalar (n) && ~isscalar (x) && ~isequal (size (n), size (x)))
    error ('inputs must be same size or scalar')
  end

  L0 = ones (size (x), class(x));
  L1 = 1 - x;

  if (isscalar (n))
    if (n == 0)
      L = L0;
    elseif (n == 1)
      L = L1;
    else
      for k = 2:n
        L = (2*k-1-x)/k .* L1 - (k-1)/k * L0;
        L0 = L1;
        L1 = L;
      end
    end
  else
    L = L0;
    L(n >= 1) = L1(n >= 1);
    maxn = max (n(:));
    for k = 2:maxn
      I = (n >= k);  % mask for entries still to be updated
      L(I) = (2*k - 1 - x(I))/k .* L1(I) - (k - 1)/k * L0(I);
      L0 = L1;
      L1 = L;
    end
  end

end

%!error laguerreL (1)
%!error laguerreL (1, 2, 3)

%!assert (isequal (laguerreL (0, rand), 1))

%!test
%! x = rand;
%! assert (isequal (laguerreL (1, x), 1 - x))

%!test
%! x=rand;
%! y1=laguerreL(2, x);
%! p2=[.5 -2 1];
%! y2=polyval(p2,x);
%! assert(y1 - y2, 0, 10*eps);

%!test
%! x=rand;
%! y1=laguerreL(3, x);
%! p3=[-1/6 9/6 -18/6 1];
%! y2=polyval(p3,x);
%! assert(y1 - y2, 0, 20*eps);

%!test
%! x=rand;
%! y1=laguerreL(4, x);
%! p4=[1/24 -16/24 72/24 -96/24 1];
%! y2=polyval(p4,x);
%! assert(y1 - y2, 0, 30*eps)

%!error <positive integer> laguerreL(1.5, 10)
%!error <same size or scalar> laguerreL([0 1], [1 2 3])
%!error <same size or scalar> laguerreL([0 1], [1; 2])

%!test
%! % numerically stable implementation (in n)
%! L = laguerreL (10, 10);
%! Lex = 1763/63;
%! assert (L, Lex, -eps)
%! L = laguerreL (20, 10);
%! Lex = -177616901779/14849255421;  % e.g., laguerreL(sym(20),10)
%! assert (L, Lex, -eps)

%!test
%! % vectorized x
%! L = laguerreL (2, [5 6 7]);
%! Lex = [3.5 7 11.5];
%! assert (L, Lex, eps)

%!test
%! L = laguerreL (0, [4 5]);
%! assert (L, [1 1], eps)

%!test
%! % vector n
%! L = laguerreL ([0 1 2 3], [4 5 6 9]);
%! assert (L, [1 -4 7 -26], eps)

%!test
%! % vector n, scalar x
%! L = laguerreL ([0 1 2 3], 6);
%! assert (L, [1 -5 7 1], eps)

%!assert (isa (laguerreL (0, single (1)), 'single'))
%!assert (isa (laguerreL (1, single ([1 2])), 'single'))
%!assert (isa (laguerreL ([1 2], single ([1 2])), 'single'))


function h = a2h(A);

% H = a2h(ABCD)
%
% ABCD to Hybrid-H transformation
% only for 2x2 matrices

d = A(1,1)*A(2,2) - A(1,2)*A(2,1);

while abs(A(2,2)) < 1e-8
    A(2,2) = A(2,2)*(1+rand*1e-8);
end;

h(1,1) = A(1,2)/A(2,2);
h(1,2) = d/A(2,2);
h(2,1) = -1/A(2,2);
h(2,2) = A(2,1)/A(2,2);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inverse dynamics for the 2dof planar robot
%
%   tau = inversedynamics_2dofplanar_fast(robot, q, qd, qdd, fe)
%
%   Where robot stores the kinematic and dynamic parameters for this robot.
%   q: joint positions.
%   qd: joint velocities.
%   qdd: joint accelerations.
%   fe: vector of external forces. (unused)
%
%   This function just executes the inverse dynamic model for this robot.
%   The equations to compute this dynamic model can be found in:
%   "ROBOT ANALYSIS. The mechanics of Serial and Parallel
%        manipulators". Lung Weng Tsai. John Wiley and Sons, inc. ISBN:
%        0-471-32593-7. page 405.
%
%   Author: Arturo Gil Aparicio arturo.gil@umh.es
%   Date: 08/03/2014
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function tau = inversedynamics_2dofplanar_fast(robot, q, qd, qdd, fe)

fprintf('\nComputing inverse dynamics for the %s robot', robot.name);

a = eval(robot.DH.a);
a1=a(1);
a2=a(2);

gc=9.81;

m1=robot.dynamics.masses(1);
m2=robot.dynamics.masses(2);

%express it as a general dynamic function
%M*qdd + V + G = Q, where Q is a vector of generalized forces or moments
% in this case, it is done just to represent the equations more clearly.
% have a look at the directdynamics_2dofplanar_fast function to observe
% this expression used to compute the direct dynamic model

%M is a 2x2 manipulator inertia matrix
M = [(m1*(1/3) + m2)*a1^2 + m2*a1*a2*cos(q(2)) + (1/3)*m2*a2^2 ...
      (1/2)*m2*a1*a2*cos(q(2))+(1/3)*m2*a2^2 ;
      (1/2)*m2*a1*a2*cos(q(2))+(1/3)*m2*a2^2  (1/3)*m2*a2^2];

V=[-m2*a1*a2*sin(q(2))*(qd(1)*qd(2)+(1/2)*qd(2)^2);
    (1/2)*m2*a1*a2*sin(q(2))*qd(1)^2];

G=[gc*(((1/2)*m1+m2)*a1*cos(q(1)) + (1/2)*m2*a2*cos(q(1)+q(2)));
    (1/2)*m2*gc*a2*cos(q(1)+q(2))];

tau = M*qdd + V + G;


function [W] = winding_number(V,F,O,varargin)
  % WINDING_NUMBER Compute the sum of solid angles of a triangle (tetrahedron)
  % described by points (vectors) V
  %
  % [W] = winding_number(V,F,O)
  % [W] = winding_number(V,F,O,'ParameterName',ParameterValue, ...)
  %
  % Inputs:
  %  V  #V by 3 list of vertex positions
  %  F  #F by 3 list of triangle indices
  %  O  #O by 3 list of origin positions
  %  Optional inputs:
  %    'Hierarchical'  followed by true or false. Use hierarchical evaluation.
  %      for mex: {true}, for matlab this is not supported
  %    'Fast' followed by whether to use fast-winding-number {false}
  %    'RayCast' followed by true or flase. Use ray cast version of approximate
  %      evaluation: {false}
  %    'TwoDRays' followed by true or false. Use 2d rays only.
  %    'NumRays' followed by the number of rays to cast for each origin
  % Outputs:
  %  W  no by 1 list of winding numbers
  %

  warning('not mex...');
  S = solid_angle(V,F,O);
  W = sum(S,2);
  switch size(F,2)
  case 3
    W = W/(2*pi);
  case 4
    W = W/(4*pi);
  end

end


function a = tanh(a)
%TANH         Gradient hyperbolic tangent tanh(a)
%

% written  10/16/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    iaccelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 08/26/12     S.M. Rump  global variables removed
%

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

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  ax = 1 ./ sqr(cosh(full(a.x(:))));
  a.x = tanh(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
      else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  if rndold
    setround(rndold)
  end


function w = quadwts(n)
%QUADWTS   Quadrature weights for Chebyshev points of 2nd kind.
%   QUADWTS(N) returns the N weights for Clenshaw-Curtis quadrature on 2nd-kind
%   Chebyshev points.
%
% See also CHEBPTS, BARYWTS.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DEVELOPER NOTE:
% We use a variant of Waldvogel's algorithm [1], due to Nick Hale. (See below)
% We note this is similar to Greg Von Winkel's approach, which can be found on
% the MathWorks File Exchange.
%
% Let $f(x) = \sum_{k=0}^nc_kT_k(x)$, then\vspace*{-3pt} }
%   I(f) = v.'*c
% where
%   v = \int_{-1}^1T_k(x)dx = { 2/(1-k^2) : k even
%                             { 0         : k odd
%     = v'*inv(TT)*f(x) where TT_{j,k} = T_k(x_j)
%     = (inv(TT)'*v)'*f(x)
% Therefore
%   I(f) = w.'f(x) => w = inv(TT).'*v;
% Here inv(TT).' = inv(TT) is an inverse discrete cosine transform of Type I.
%
% Furthermore, since odd entries in v are zero, can compute via FFT without
% doubling up from N to 2N (though we still need to double up from N/2 to N to
% facilitate the use of ifft).
%
% References:
%   [1] Joerg Waldvogel, "Fast construction of the Fejer and Clenshaw-Curtis
%       quadrature rules", BIT Numerical Mathematics 46 (2006), pp 195-202.
%   [2] Greg von Winckel, "Fast Clenshaw-Curtis Quadrature",
%       http://www.mathworks.com/matlabcentral/fileexchange/6911, (2005)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ( n == 0 )                      % Special case (no points!)
    w = [];
elseif ( n == 1 )                  % Special case (single point)
    w = 2;
else                               % General case
    c = 2./[1, 1-(2:2:(n-1)).^2];  % Exact integrals of T_k (even)
    c = [c, c(floor(n/2):-1:2)];   % Mirror for DCT via FFT
    w = ifft(c);                   % Interior weights
    w([1,n]) = w(1)/2;             % Boundary weights
end

end


%DEMO_NEURALNETCOV  Demonstration of Gaussian process with a neural
%                   network covariance function
%
%  Description
%    Infinite neural network solutions in 2D and 1D regression
%    problems with a comparison to Gaussian process solution given
%    by squared exponential covariance function. The noisy
%    observations y are assumed to satisfy
%
%         y = f + e,    where e ~ N(0, s^2)
%
%    where f is an unknown underlying function. A zero mean
%    Gaussian process prior is assumed for f
%
%         f ~ N(0, K),
%
%    where K is the covariance matrix whose elements are given by
%    neural network (or squared exponential) covariance function. A
%    prior is assumed for parameters of the covariance functions,
%    and the inference is done with a MAP estimate for parameter
%    values.
%
%    For more detailed discussion of infinite neural networks, see
%    e.g.
%
%      Neal, R. M. (1996). Bayesian Learning for Neural Networks.
%      Springer-Verlag.
%
%      Williams, C. K. I. (1996). Computing with infinite networks.
%      In Advances in Neural Information Processing Systems 9. MIT
%      Press, Cambridge, MA.
%
%
%  See also DEMO_REGRESSION1

% Copyright (c) 2010 Jaakko Riihim�ki, Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% 2D REGRESSION DATA

% create 2D example data
x=rand(300,2)*2-1;
y=zeros(size(x,1),1); y(x(:,1)>0&x(:,2)>0)=1;
y=y+0.1*randn(size(y));

[n, nin] = size(x);

% --- Construct the model ---

% squared exponential covariance function
gpcf1 = gpcf_sexp('lengthScale', ones(1,nin), 'magnSigma2', 1);
% neural network covariance function
gpcf2 = gpcf_neuralnetwork('weightSigma2', ones(1,nin), 'biasSigma2', 1);
% Gaussian noise structures
lik = lik_gaussian('sigma2', 0.2^2);

% a prior structure for GP parameters
pt = prior_t('s2', 4);
gpcf1 = gpcf_sexp(gpcf1, 'lengthScale_prior', pt, 'magnSigma2_prior', pt);
gpcf2 = gpcf_neuralnetwork(gpcf2, 'weightSigma2_prior', pt, 'biasSigma2_prior', pt);

gp = gp_set('lik', lik, 'cf', gpcf1);
gp2 = gp_set('lik', lik, 'cf', gpcf2);

% --- MAP estimate using scaled conjugate gradient algorithm ---
%     (see scg for more details)

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);
gp2=gp_optim(gp2,x,y,'opt',opt);

% create points where predictions are made
[xt1,xt2]=meshgrid(-1.5:0.05:1.5,-1.5:0.05:1.5);
xt=[xt1(:) xt2(:)];
% compute the predictions
[Eft_map, Varft_map] = gp_pred(gp, x, y, xt);
[Eft_map2, Varft_map2] = gp_pred(gp2, x, y, xt);

% Plot the predictions and data
figure, set(gcf, 'color', 'w')
mesh(xt1, xt2, reshape(Eft_map,size(xt1,1),size(xt1,2)));
hold on
plot3(x(:,1), x(:,2), y, '*')
axis on;
title('GP (squared exponential) predictions and the data points');

figure, set(gcf, 'color', 'w')
mesh(xt1, xt2, reshape(Eft_map2,size(xt1,1),size(xt1,2)));
hold on
plot3(x(:,1), x(:,2), y, '*')
axis on;
title('GP (neural network) predictions and the data points');

% 1D REGRESSION DATA

% create a 1D toy data
x=rand(100,1)*4-2;
y=norm_pdf(4*x)+0.05*randn(size(x));
[n, nin] = size(x);

gpcf1 = gpcf_sexp('lengthScale', ones(1,nin), 'magnSigma2', 1);
gpcf2 = gpcf_neuralnetwork('weightSigma2', ones(1,nin), 'biasSigma2', 1);

gpcf1 = gpcf_sexp(gpcf1, 'lengthScale_prior', pt, 'magnSigma2_prior', pt);
gpcf2 = gpcf_neuralnetwork(gpcf2, 'weightSigma2_prior', pt, 'biasSigma2_prior', pt);
lik = lik_gaussian();

gp = gp_set('lik', lik, 'cf', gpcf1);
gp2 = gp_set('lik', lik, 'cf', gpcf2);

% --- MAP estimate using scaled conjugate gradient algorithm ---
%     (see fminscg for more details)

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);
gp2=gp_optim(gp2,x,y,'opt',opt);

% create points where predictions are made
xgrid=linspace(min(x)-1.5,max(x)+1.5,200)';
[Eft_map, Varft_map, lpyt_map, Eyt_map, Varyt_map] = gp_pred(gp, x, y, xgrid, 'yt', ones(200,1));
[Eft_map2, Varft_map2, lpyt_map2, Eyt_map2, Varyt_map2] = gp_pred(gp2, x, y, xgrid, 'yt', ones(200,1));

% Plot the predictions and data
color1=ones(1,3)*0.8; color2=ones(1,3)*0.5;
figure, set(gcf, 'color', 'w'), hold on
h1=fill([xgrid' fliplr(xgrid')], [(Eyt_map+1.96*sqrt(Varyt_map))' fliplr((Eyt_map-1.96*sqrt(Varyt_map))')], color1, 'edgecolor', color1);
% GP mean
h2=plot(xgrid, Eyt_map, 'color', color2, 'linewidth', 3);
% observations
h3=plot(x, y, 'xk', 'markersize', 10, 'linewidth', 2);
% true function
h4=plot(xgrid, norm_pdf(4*xgrid), 'color', 'r', 'linewidth', 2);
legend([h1 h2 h3 h4], 'GP 95% CI', 'GP mean', 'observations', 'true latent function')
title('GP (squared exponential) predictions and the data points');

figure, set(gcf, 'color', 'w'), hold on
h1=fill([xgrid' fliplr(xgrid')], [(Eyt_map2+1.96*sqrt(Varyt_map2))' fliplr((Eyt_map2-1.96*sqrt(Varyt_map2))')], color1, 'edgecolor', color1);
% GP mean
h2=plot(xgrid, Eyt_map2, 'color', color2, 'linewidth', 3);
% observations
h3=plot(x, y, 'xk', 'markersize', 10, 'linewidth', 2);
% true function
h4=plot(xgrid, norm_pdf(4*xgrid), 'color', 'r', 'linewidth', 2);
legend([h1 h2 h3 h4], 'GP 95% CI', 'GP mean', 'observations', 'true latent function')
title('GP (neural network) predictions and the data points');


function [lambda, r2] = duration_amplitude(self, law, min_amplitude, mag_zone)
    %DURATION_AMPLITUDE Use the duration-amplitde
    %Compute the fraction of a data series above each
    %amplitude level, plot the duration vs amplitude data, and then
    %allow user input to fit a regression line.
    %   rsamObject.duration_amplitude(law, min_amplitude)
    %
    %   Inputs:
    %       law = 'exponential' or 'power'
    %       min_amplitude = (Optional) the smallest amplitude to use on
    %       the x-axis. Otherwise will be 10^10 times smallest than the
    %       largest amplitude.
    %
    %   Outputs:
    %       (None) A graph is plotted, the user clicks two points, and
    %       from that slope, the characteristic amplitude is computed
    %       and shown on the screen.

    y = self.data;
    n = length(y);
    a = abs(y);
    max_amplitude = max(a);
    if ~exist('min_amplitude', 'var')
        min_amplitude = max([min(a) max(a)*1e-10]);
    end

    % Method 1
    index=0;
    x = min_amplitude;
    while x < max_amplitude,
        i = find(a>x);
        f = length(i);
        index = index+1;
        frequency(index) = f;
        threshold(index) = x;
        x = x * 1.2;
    end
    clear x y a  f  n  min_amplitude max_amplitude index ;

    %             % Method 2
    %             threshold = [0.0 logspace(min_amplitude, max_amplitude, 50)];
    %             nsamples=[];
    %             for d = 1:length(threshold)
    %                 i = find(a > threshold(d));
    %                 frequency(d) = length(i)/length(y);
    %             end
    %             clear d, nsamples, i, y

    %% PLOT_DURATION_AMPLITUDE
    % plot graph, user select two points, compute
    % characteristic from slope.
    % Use different method depending on whether it is a
    % power law or exponential.

    % define x and y
    switch law
        case {'exponential'}
            x = threshold;
            xlabelstr = 'RMS Displacement(nm)';
        case {'power'}
            x = log10(threshold);
            xlabelstr = 'log10(RMS Displacement(nm))';
        otherwise
            error('law unknown')
    end
    y=log10(frequency);

    % plot duration-amplitude data as circles
    figure
    plot(x,y,'o');
    xlabel(xlabelstr);
    ylabel('log10(Cumulative Minutes)');
    hold on;
    %set(gca,'XLim',[xmin xmax]);

    lambda=0;
    r2=0;

    % check if we have pre-set the magnitude range, effectively
    % our x1 and x2 click points with ginput
    if exist('mag_zone','var') % no user select
        switch law
            case {'power'}
                x1=min(mag_zone);
                x2=max(mag_zone);
            case {'exponential'}
                x1=10^min(mag_zone);
                x2=10^max(mag_zone);

            otherwise
                error('law unknown')
        end
        if x1<min(x)
            x2=min(x);
        end
        y1=interp1(x,y,x1);
        if x2>max(x)
            x2=max(x);
        end
        y2=interp1(x,y,x2);

        % draw a dotted line to show where user selected
        %plot([x1 x2], [y1 y2], '-.');

        % select requested data range and do a least squares fit
        ii = find(x >= x1 & x <= x2);
        wx = x(ii);
        wy = y(ii);
        [p,S]=polyfit(wx,wy,1);
        yfit = polyval(p,wx);
        thiscorr = corrcoef(wy, yfit)
        %try
        if numel(thiscorr)>1
            r2 = thiscorr(1,2);

            % compute lambda
            switch law
                case {'exponential'}
                    lambda = -p(1)/log10(exp(1));
                case {'power'}
                    lambda = -p(1);
                otherwise
                    error('law unknown')
            end

            disp(sprintf('characteristic D_R_S=%.2f cm^2, R^2=%.2f',lambda,r2));

            % draw the fitted line
            xf = [min(wx) max(wx)];
            yf = xf * p(1) + p(2);
            plot(xf, yf,'-');

            %ylabel('log10(t/t0)');
            %xlabel(sprintf('D_R_S (%s) (cm^2)',measure));

            % Add legend
            yrange=get(gca,'YLim');
            xlim = get(gca,'XLim');
            xmax=max(xlim);

            xpos = xmax*0.65;
            ypos = (yrange(2)-yrange(1))*0.8;
            r2str=sprintf('%.2f',r2);
            lambdastr=sprintf('%.2f',lambda);
            if strcmp(law,'exponential')
                tstr = [' \lambda=',lambdastr,' R^2=',r2str];
            else
                tstr = [' \gamma=',lambdastr,' R^2=',r2str];
            end

            text(xpos, ypos, tstr, ...
                'FontName','Helvetica','FontSize',[14],'FontWeight','bold');
        else
            lambda=NaN;
            r2=NaN;
        end

    else

        % user select a range of data
        disp('Left-click Select lowest X, any other mouse button to ignore this station')
        [x1, y1, button1]=ginput(1);
        if button1==1
            disp('Left-click Select highest X, any other mouse button to ignore this station')
            [x2, y2, button2]=ginput(1);
            if button2==1
                if x2>x1
                   % draw a dotted line to show where user selected
                    plot([x1 x2], [y1 y2], '-.');

                    % select requested data range and do a least squares fit
                    ii = find(x >= x1 & x <= x2);
                    wx = x(ii);
                    wy = y(ii);
                    [p,S]=polyfit(wx,wy,1);
                    yfit = polyval(p,wx);
                    thiscorr = corrcoef(wy, yfit)

                    r2 = thiscorr(1,2);

                    % compute lambda
                    switch law
                        case {'exponential'}
                            lambda = -p(1)/log10(exp(1));
                        case {'power'}
                            lambda = -p(1);
                        otherwise
                            error('law unknown')
                    end

                    disp(sprintf('characteristic D_R_S=%.2f cm^2, R^2=%.2f',lambda,r2));

                    % draw the fitted line
                    xf = [min(wx) max(wx)];
                    yf = xf * p(1) + p(2);
                    plot(xf, yf,'-');

                    %ylabel('log10(t/t0)');
                    %xlabel(sprintf('D_R_S (%s) (cm^2)',measure));

                    % Add legend
                    yrange=get(gca,'YLim');
                    xlim = get(gca,'XLim');
                    xmax=max(xlim);

                    xpos = xmax*0.65;
                    ypos = (yrange(2)-yrange(1))*0.8;
                    r2str=sprintf('%.2f',r2);
                    lambdastr=sprintf('%.2f',lambda);
                    if strcmp(law,'exponential')
                        tstr = [self.sta,' \lambda=',lambdastr,' R^2=',r2str];
                    else
                        tstr = [self.sta,' \gamma=',lambdastr,' R^2=',r2str];
                    end

                    text(xpos, ypos, tstr, ...
                        'FontName','Helvetica','FontSize',[14],'FontWeight','bold');

                end
            end
        end
    end
end


function r = plus(p,q)
% MEAS/PLUS  Implement p + q for meas.

% make a meas called r
r = meas();

% give it the right entries
r.value = p.value + q.value;
r.error = sqrt(p.error^2 + q.error^2);


% op_phaseAlignAverages_fd.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% [out,phs]=op_phaseAlignAverages_fd(in,minppm,maxppm,Npts,avg,weighting)
%
% DESCRIPTION:
% Perform time-domain spectral registration using a limited range of
% frequencies and using only phase adjustment (no frequency adjustment).
% This is rarely used.
%
% INPUTS:
% in         = Input data structure.
% minppm     = Minimum of frequency range (ppm).
% maxppm     = Maximum of frequnecy range (ppm).
% Npts       = Number of points in time domain to use for alignment.
% avg        = Align averages to the average of the averages ('y'), or the
%              first average in the series ('n');
% weighting	 = (Optional) Apply less weight to the later points of the fid?
%
% OUTPUTS:
% out        = Output following alignment of averages.
% phs        = Vector of phases (in degrees) used for alignment.

function [out,phs]=op_phaseAlignAverages_fd(in,minppm,maxppm,Npts,avg,weighting)

if ~in.flags.addedrcvrs
    error('ERROR:  I think it only makes sense to do this after you have combined the channels using op_addrcvrs.  ABORTING!!');
end

if nargin<6
    weighting='n';
end

if in.dims.subSpecs==0
    B=1;
else
    B=in.sz(in.dims.subSpecs);
end

phs=zeros(in.sz(in.dims.averages),B);
in_avg=op_averaging(in);
fids=zeros(size(in.fids));
if weighting=='y' || weighting=='Y'
    temp=op_freqrange(in_avg,minppm,maxppm);
    wgt_func=abs(temp.fids).^2;
else
    temp=op_freqrange(in_avg,minppm,maxppm);
    wgt_func=ones(size(temp.fids));
end

for m=1:B
    if avg=='y'||avg=='Y'
        base=in_avg;
        base=op_freqrange(base,minppm,maxppm);
        base=base.fids(1:Npts,m);
        begin=1;
    else
        base=op_freqrange(in,minppm,maxppm);
        base=base.fids(1:Npts,1,m);
        begin=2;
        fids(:,1,m)=in.fids(:,1,m);
    end
    for n=begin:in.sz(in.dims.averages)
        datarange=op_freqrange(in,minppm,maxppm);
        phsdiffs=(phase(base)-phase(datarange.fids(1:Npts,n,m)))*180/pi;
        phs(n,m)=mean(phsdiffs.*wgt_func(1:Npts,m))/mean(wgt_func(1:Npts,m));
        fids(:,n,m)=addphase(in.fids(:,n,m),phs(n,m));
    end
end

%re-calculate Specs using fft
specs=fftshift(ifft(fids,[],in.dims.t),in.dims.t);

%FILLING IN DATA STRUCTURE
out=in;
out.fids=fids;
out.specs=specs;

%FILLING IN THE FLAGS
out.flags=in.flags;
out.flags.writtentostruct=1;
out.flags.freqcorrected=1;

end


function Vf = fillMissingGrid(xGridV, yGridV, zGridV, Vf)
% function matrixOut = fillMissingGrid(xGridV, yGridV, zGridV, Vf)
%
% This function takes in a matrix that contains NaN values and replaces
% them with values that are linearly interpolated from the surrounding.
%
% APA, 10/03/2012

% Find NaN locations
indNaN = isnan(Vf(:,:,:,1));

% Find Not NaN locations
indNotNaN = ~isnan(Vf(:,:,:,1));

% Create x,y,z 3D grid
[xM,yM,zM] = meshgrid(xGridV, yGridV, zGridV);

% Get x,y,z of NaN points
xNaNv = xM(indNaN);
yNaNv = yM(indNaN);
zNaNv = zM(indNaN);

% Get x,y,z of Not-NaN points
xNotNaNv = xM(indNotNaN);
yNotNaNv = yM(indNotNaN);
zNotNaNv = zM(indNotNaN);

% Get values at Not-NaN points
xVf = Vf(:,:,:,1);
xVfv = xVf(indNotNaN);
yVf = Vf(:,:,:,2);
yVfv = yVf(indNotNaN);
zVf = Vf(:,:,:,3);
zVfv = zVf(indNotNaN);

Fx = TriScatteredInterp([xNotNaNv yNotNaNv zNotNaNv],double(xVfv));
Fy = TriScatteredInterp([xNotNaNv yNotNaNv zNotNaNv],double(yVfv));
Fz = TriScatteredInterp([xNotNaNv yNotNaNv zNotNaNv],double(zVfv));

xVfNaN = Fx([xNaNv yNaNv zNaNv]);
yVfNaN = Fy([xNaNv yNaNv zNaNv]);
zVfNaN = Fz([xNaNv yNaNv zNaNv]);

% xVfNaN = zeros(1,length(xNaNv));
% yVfNaN = xVfNaN;
% zVfNaN = xVfNaN;
% numVoxels = length(xNaNv);
%
% for i=1:numVoxels
%     disp([num2str(i),' / ', num2str(numVoxels)])
%     %     % Find distance of NaN from all the Not-NaN points
%     %     distV = (xNaNv(i) - xNotNaNv).^2 + (yNaNv(i) - yNotNaNv).^2 + (zNaNv(i) - zNotNaNv).^2;
%     %     % Sort by distance
%     %     [~,indV] = sort(distV);
%
%     %     indKrig = indV(1:20);
%     indKrig = [];
%     % Search in neighborhood
%     searchRadius = 0.2;
%     while sum(indKrig>0) < 5
%         indKrig = (xNaNv(i) - xNotNaNv).^2 + (yNaNv(i) - yNotNaNv).^2 + (zNaNv(i) - zNotNaNv).^2 <= searchRadius^2;
%         searchRadius = searchRadius + 0.1;
%     end
%     % Kriging
%     dmodelX = dacefit([xNotNaNv(indKrig) yNotNaNv(indKrig) zNotNaNv(indKrig)], double(xVfv(indKrig)), @regpoly0, @correxp, 10, 1e-1, 20);
%     xVfKrig = predictor([xNaNv(i) yNaNv(i) zNaNv(i)], dmodelX);
%     dmodelY = dacefit([xNotNaNv(indKrig) yNotNaNv(indKrig) zNotNaNv(indKrig)], double(yVfv(indKrig)), @regpoly0, @correxp, 10, 1e-1, 20);
%     yVfKrig = predictor([xNaNv(i) yNaNv(i) zNaNv(i)], dmodelY);
%     dmodelZ = dacefit([xNotNaNv(indKrig) yNotNaNv(indKrig) zNotNaNv(indKrig)], double(zVfv(indKrig)), @regpoly0, @correxp, 10, 1e-1, 20);
%     zVfKrig = predictor([xNaNv(i) yNaNv(i) zNaNv(i)], dmodelZ);
%
%     xNotNaNv = [xNotNaNv; xNaNv(i)];
%     yNotNaNv = [yNotNaNv; yNaNv(i)];
%     zNotNaNv = [zNotNaNv; zNaNv(i)];
%     xVfv = [xVfv; xVfKrig];
%     yVfv = [yVfv; yVfKrig];
%     zVfv = [zVfv; zVfKrig];
%
%     xVfNaN(i) = xVfKrig;
%     yVfNaN(i) = yVfKrig;
%     zVfNaN(i) = zVfKrig;
%
% end

xVf(indNaN) = xVfNaN;
yVf(indNaN) = yVfNaN;
zVf(indNaN) = zVfNaN;
Vf(:,:,:,1) = xVf;
Vf(:,:,:,2) = yVf;
Vf(:,:,:,3) = zVf;


function [varargout] = likGaussWarpExact(warp, hyp, y, mu, varargin)

% likGaussWarp - Warped Gaussian likelihood for regression.
% The expression for the likelihood is
%   likGaussWarp( y | t ) = likGauss( g(y) | t ) * g'(y),
% where likGauss is the Gaussian likelihood and g is the warping function.
%
% The hyperparameters are:
%
% hyp = [ theta_1
%         theta_2
%           ..
%         theta_ng
%         log(sn) ]
%
% Here, sn is the standard deviation of the underlying Gaussian and theta_i for
% i=1..ng are the ng hyperparameters of the warping function g.
%
% At the moment, likGaussWarp offers 3 different warping functions:
% id                   yields g(y) = y  =>  likGaussWarp = likGauss
% poly<m> e.g. 'poly1' yields g(y) = y  =>  likGaussWarp = likGauss
%              'poly3' yields g(y) = y + c1*sy*ay^2 + c2*sy*ay^3
%                             where sy = sign(y), ay = abs(y), cj = exp(theta_j)
% tanh<m> e.g. 'tanh0' yields g(y) = y  =>  likGaussWarp = likGauss
%              'tanh2' yields g(y) = y + a1*tanh(b1*(y+c1)) + a2*tanh(b2*(y+c2))
%                 where aj = exp(theta_j), bj = exp(theta_j+m), bj = theta_j+2*m
%
% The code is based on the exposition in the paper Warped Gaussian Processes,
% NIPS, 2003 by Edward Snelson, Carl Rasmussen and Zoubin Ghahramani.
%
% Several modes are provided, for computing likelihoods, derivatives and moments
% respectively, see likFunctions.m for the details. In general, care is taken
% to avoid numerical issues when the arguments are extreme.
%
% Copyright (c) by Hannes Nickisch, 2013-10-24.
%
% See also LIKFUNCTIONS.M.

lik = {@likGauss}; % in principle any likelihood function can be warped but only
% for homoscedastic likelihoods, in particular Gaussian has feasible integrals
if numel(warp)==0, warp = 'id'; end               % set default warping function
if ischar(warp) || isa(warp,'function_handle'); warp = {warp}; end

ng = feval(warp{:});        % number of hyperparameters for the warping function
nhyp = ['(',num2str(ng),'+',feval(lik{:}),')'];      % number of hyperparameters
if nargin<4, varargout = {nhyp}; return, end       % report number of parameters
nhyp = eval(nhyp);
if nhyp>length(hyp), error('not enough hyperparameters'), end

[gy,lgpy] = feval(warp{:},y,hyp(1:ng));                     % evaluate warping function

% [gy,ig(warp,gy,hyp(1:ng))]'
i = 0; if nargin>6, i = varargin{3}; varargin{3} = varargin{3}-ng; end
varargout = cell(nargout,1);              % allocate memory for output arguments
if i==0 || ng<i                               % only evaluate the required parts
  [varargout{:}] = feval(lik{:},hyp(ng+1:end),gy,mu,varargin{:});     % eval lik
end
if nargin<6                              % prediction mode if inf is not present
  if numel(y)==0,  y = zeros(size(mu)); end
  s2zero = 1; if nargin>4, s2 = varargin{1}; end                       % s2==0 ?
  if nargin>4&&numel(s2)>0&&norm(s2)>eps>0, s2zero = 0; end
  if s2zero                                                    % log probability
    lp = likGaussWarpExact(warp, hyp, y, mu, [], 'infLaplace'); s2 = 0*mu;
  else
    lp = likGaussWarpExact(warp, hyp, y, mu, s2, 'infEP');               % prediction
  end
  if nargout>0, varargout{1} = lp; end                       % works for any lik
  % the predictive moments are very hard to compute for lik not being likGauss
  if nargout>1
    ymu = mu;                                                % first g(y) moment
    sn2 = exp(2*hyp(ng+1));                            % Gaussian noise variance
    ys2 = s2 + sn2;                                         % second g(y) moment
%     ymuM = ig(warp,ymu,hyp(1:ng));                                    % median
%     yupp = ig(warp,ymu+2*sqrt(ys2),hyp(1:ng));       % 95% confidence interval
%     ylow = ig(warp,ymu-2*sqrt(ys2),hyp(1:ng));
%     ys2C = (yupp-ylow).^2/16;

%    N = 20; [t,w] = gauher(N); oN = ones(1,N);     % Gaussian-Hermite quadrature
%    Z = sqrt(ys2(:))*t'+ymu(:)*oN;
%    Y = ig(warp,Z,hyp(1:ng));
%    ymu = Y*w; ys2 = (Y-ymu*oN).^2*w;                % first and second y moment

    ymu = feval(warp{:},mu,hyp(1:ng),'inv');
    % muin = feval(warp{:},mu,hyp(1:ng));
    %ylow = feval(warp{:},muin-10*sqrt(ys2),hyp(1:ng),'inv');
    ylow = feval(warp{:},mu-sqrt(ys2),hyp(1:ng),'inv');
    ys2 = (ymu-ylow).^2;

    varargout{2} = reshape(ymu,size(mu));
    if nargout>2
      varargout{3} = ys2;
    end
  end

else
  inf = varargin{2};                                    % obtain input variables
  switch inf
  case {'infLaplace','infEP'}                     % they have the same structure
    if nargin<7                                             % no derivative mode
      if nargout>0, varargout{1} = varargout{1} + lgpy; end
    else                                                       % derivative mode
      if i<=ng                  % derivatives w.r.t. warping function parameters
        n = max([numel(y),numel(mu)]);
        for j=2:nargout, varargout{j} = zeros(n,1); end
        [dgy,dlgpy] = feval(warp{:},y,hyp(1:ng),i);    % warping function derivative
        out = cell(nargout+1,1);                               % allocate memory
        [out{:}] = likGaussWarpExact(warp, hyp, y, mu, varargin{1:2});    % query lik
        % works only for homoscedastic likelihoods where y and mu can be swapped
        if nargout>0, varargout{1} = dlgpy - out{2}.*dgy; end % apply chain rule
        if nargout>1, varargout{2} =       - out{3}.*dgy; end
        if nargout>2, varargout{3} =       - out{4}.*dgy; end
      end
    end

  case 'infVB'           % output does not depend on mu and following parameters
  end
end


function nwspgr_size_test ( )

%*****************************************************************************80
%
%% NWSPGR_SIZE_TEST tests NWSPGR_SIZE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 January 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'NWSPGR_SIZE_TEST:\n' );
  fprintf ( 1, '  NWSPGR_SIZE returns the size of a sparse grid, based on:\n' );
  fprintf ( 1, '  one of the built-in 1D rules, or a family of 1D rules\n' );
  fprintf ( 1, '  supplied by the user.\n' );

  d = 2;
  k = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Kronrod-Patterson, [0,1], Dim %d, Level %d Symmetric\n', d, k );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Full          %4d\n', nwspgr_size ( 'kpu', d, k, 1, 0 ) );
  fprintf ( 1, '  Compressed    %4d\n', nwspgr_size ( 'kpu', d, k, 1, 1 ) );

  d = 2;
  k = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Kronrod-Patterson, (-oo,+oo), Dim %d, Level %d Symmetric\n', d, k );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Full          %4d\n', nwspgr_size ( 'kpn', d, k, 1, 0 ) );
  fprintf ( 1, '  Compressed    %4d\n', nwspgr_size ( 'kpn', d, k, 1, 1 ) );

  d = 2;
  k = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Gauss-Legendre, [0,1], Dim %d, Level %d Symmetric\n', d, k );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Full          %4d\n', nwspgr_size ( 'gqu', d, k, 1, 0 ) );
  fprintf ( 1, '  Compressed    %4d\n', nwspgr_size ( 'gqu', d, k, 1, 1 ) );

  d = 2;
  k = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Gauss Hermite, (-oo,+oo), [0,1], Dim %d, Level %d Symmetric\n', d, k );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Full          %4d\n', nwspgr_size ( 'gqn', d, k, 1, 0 ) );
  fprintf ( 1, '  Compressed    %4d\n', nwspgr_size ( 'gqn', d, k, 1, 1 ) );

  d = 2;
  k = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Clenshaw Curtis, [-1,+1], [0,1], Dim %d, Level %d Unsymmetric\n', d, k );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Full        %4d\n', nwspgr_size ( 'cce', d, k, 0, 0 ) );
  fprintf ( 1, '  Compressed  %4d\n', nwspgr_size ( 'cce', d, k, 0, 1 ) );
%
%  Do a table.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Dimension / Level table for Clenshaw Curtis Exponential (CCE) Compressed\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dim:  ' )
  for d = 1 : 10
    fprintf ( 1, '  %6d', d );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Level:\n' );
  for k = 1 : 6
    fprintf ( 1, '  %2d:  ', k );
    for d = 1 : 10
      fprintf ( 1, '  %6d', nwspgr_size ( 'cce', d, k, 0, 1 ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Do a table.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Dimension / Level table for Clenshaw Curtis Exponential (CCE) Uncompressed\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dim:  ' )
  for d = 1 : 10
    fprintf ( 1, '  %6d', d );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Level:\n' );
  for k = 1 : 6
    fprintf ( 1, '  %2d:  ', k );
    for d = 1 : 10
      fprintf ( 1, '  %6d', nwspgr_size ( 'cce', d, k, 0, 0 ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Do a GLL table.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Dimension / Level table for Gauss-Legendre Linear (GLL) Compressed\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dim:  ' )
  for d = 1 : 6
    fprintf ( 1, '  %6d', d );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Level:\n' );
  for k = 1 : 11
    fprintf ( 1, '  %2d:  ', k );
    for d = 1 : 6
      fprintf ( 1, '  %6d', nwspgr_size ( 'gqu2', d, k, 0, 1 ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Do a GLO table.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Dimension / Level table for Gauss-Legendre-Odd (GLO) Compressed\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dim:  ' )
  for d = 1 : 6
    fprintf ( 1, '  %6d', d );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Level:\n' );
  for k = 1 : 11
    fprintf ( 1, '  %2d:  ', k );
    for d = 1 : 6
      fprintf ( 1, '  %6d', nwspgr_size ( 'glo', d, k, 0, 1 ) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function bounds = computeBounds(config)
% bounds = computeBounds(config)
%
% This function returns the bounds on the decision variables, to be passed
% to the trajectory optimization solver.
%
% INPUTS:
%   config.dyn = physical parameters of the model
%       .m1 = cart mass
%       .m2 = pole mass
%       .g = gravity
%       .l = pendulum length
%
% OUTPUTS:
%   bounds = struct of bounds for decision variables:
%       .initialTime.lower = scalar
%       .initialTime.upper = scalar
%       .finalTime.lower = scalar
%       .finalTime.upper = scalar
%       .initialState.lower = [ns, 1]
%       .initialState.upper = [ns, 1]
%       .finalState.lower = [ns, 1]
%       .finalState.upper = [ns, 1]
%       .state.lower = [ns, 1]
%       .state.upper = [ns, 1]
%       .control.lower = [nc, 1]
%       .control.upper = [nc, 1]
%

% Unpack physical parameters
m1 = config.dyn.m1;
% m2 = config.dyn.m2;
g = config.dyn.g;
l = config.dyn.l;

% Boundry values
bounds.initialState.lower = [0;0;0;0];
bounds.initialState.upper = bounds.initialState.lower;
bounds.finalState.lower = [config.guess.state(1:2,end);0;0];
bounds.finalState.upper = bounds.finalState.lower;

% Bounds on duration (total trajectory time)
wn = sqrt(l/g);
bounds.initialTime.lower = 0;
bounds.initialTime.upper = 0;
bounds.finalTime.lower = 0.5*(2*pi*wn);
bounds.finalTime.upper = 4*(2*pi*wn);

% Bounds on state:
bounds.state.lower = [-1.0*l; -2*pi; -10*l/wn; -10*pi/wn];
bounds.state.upper = [1.0*l; 2*pi; 10*l/wn; 10*pi/wn];

% Bounds on control:
bounds.control.lower = -5*m1*g;
bounds.control.upper = 5*m1*g;

end


function [mu,sig,alpha,beta] = fit_eeg_distribution(X,min_clean_fraction,max_dropout_fraction,quants,step_sizes,beta)
% Estimate the mean and standard deviation of clean EEG from contaminated data.
% [Mu,Sigma,Alpha,Beta] = fit_eeg_distribution(X,MinCleanFraction,MaxDropoutFraction,FitQuantiles,StepSizes,ShapeRange)
%
% This function estimates the mean and standard deviation of clean EEG from a sample of amplitude
% values (that have preferably been computed over short windows) that may include a large fraction
% of contaminated samples. The clean EEG is assumed to represent a generalized Gaussian component in
% a mixture with near-arbitrary artifact components. By default, at least 25% (MinCleanFraction) of
% the data must be clean EEG, and the rest can be contaminated. No more than 10%
% (MaxDropoutFraction) of the data is allowed to come from contaminations that cause lower-than-EEG
% amplitudes (e.g., sensor unplugged). There are no restrictions on artifacts causing
% larger-than-EEG amplitudes, i.e., virtually anything is handled (with the exception of a very
% unlikely type of distribution that combines with the clean EEG samples into a larger symmetric
% generalized Gaussian peak and thereby "fools" the estimator). The default parameters should be
% fine for a wide range of settings but may be adapted to accomodate special circumstances.
%
% The method works by fitting a truncated generalized Gaussian whose parameters are constrained by
% MinCleanFraction, MaxDropoutFraction, FitQuantiles, and ShapeRange. The alpha and beta parameters
% of the gen. Gaussian are also returned. The fit is performed by a grid search that always finds a
% close-to-optimal solution if the above assumptions are fulfilled.
%
% In:
%   X : vector of amplitude values of EEG, possible containing artifacts
%       (coming from single samples or windowed averages)
%
%   MinCleanFraction : Minimum fraction of values in X that needs to be clean
%                      (default: 0.25)
%
%   MaxDropoutFraction : Maximum fraction of values in X that can be subject to
%                        signal dropouts (e.g., sensor unplugged) (default: 0.1)
%
%   FitQuantiles : Quantile range [lower,upper] of the truncated generalized Gaussian distribution
%                  that shall be fit to the EEG contents (default: [0.022 0.6])
%
%   StepSizes : Step size of the grid search; the first value is the stepping of the lower bound
%               (which essentially steps over any dropout samples), and the second value
%               is the stepping over possible scales (i.e., clean-data quantiles)
%               (default: [0.01 0.01])
%
%   ShapeRange : Range that the clean EEG distribution's shape parameter beta may take (default:
%                1.7:0.15:3.5)
%
% Out:
%   Mu : estimated mean of the clean EEG distribution
%
%   Sigma : estimated standard deviation of the clean EEG distribution
%
%   Alpha : estimated scale parameter of the generalized Gaussian clean EEG distribution (optional)
%
%   Beta : estimated shape parameter of the generalized Gaussian clean EEG distribution (optional)
%
%                                Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                                2013-08-15

% Copyright (C) Christian Kothe, SCCN, 2013, christiankothe@gmail.com
%
% This program is free software; you can redistribute it and/or modify it under the terms of the GNU
% General Public License as published by the Free Software Foundation; either version 2 of the
% License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
% even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% General Public License for more details.
%
% You should have received a copy of the GNU General Public License along with this program; if not,
% write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
% USA

% assign defaults
if ~exist('min_clean_fraction','var') || isempty(min_clean_fraction)
    min_clean_fraction = 0.25; end
if ~exist('max_dropout_fraction','var') || isempty(max_dropout_fraction)
    max_dropout_fraction = 0.1; end
if ~exist('quants','var') || isempty(quants)
    quants = [0.022 0.6]; end
if ~exist('step_sizes','var') || isempty(step_sizes)
    step_sizes = [0.01 0.01]; end
if ~exist('beta','var') || isempty(beta)
    beta = 1.7:0.15:3.5; end

% sanity checks
if ~isvector(quants) || numel(quants) > 2
    error('Fit quantiles needs to be a 2-element vector (support for matrices deprecated).'); end
if any(quants(:)<0) || any(quants(:)>1)
    error('Unreasonable fit quantiles.'); end
if any(step_sizes<0.0001) || any(step_sizes>0.1)
    error('Unreasonable step sizes.'); end
if any(beta>=7) || any(beta<=1)
    error('Unreasonable shape range.'); end

% sort data so we can access quantiles directly
X = double(sort(X(:)));
n = length(X);

% calc z bounds for the truncated standard generalized Gaussian pdf and pdf rescaler
for b=1:length(beta)
    zbounds{b} = sign(quants-1/2).*gammaincinv(sign(quants-1/2).*(2*quants-1),1/beta(b)).^(1/beta(b)); %#ok<*AGROW>
    rescale(b) = beta(b)/(2*gamma(1/beta(b)));
end

% determine the quantile-dependent limits for the grid search
lower_min = min(quants);                    % we can generally skip the tail below the lower quantile
max_width = diff(quants);                   % maximum width is the fit interval if all data is clean
min_width = min_clean_fraction*max_width;   % minimum width of the fit interval, as fraction of data

% get matrix of shifted data ranges
X = X(bsxfun(@plus,(1:round(n*max_width))',round(n*(lower_min:step_sizes(1):lower_min+max_dropout_fraction))));
X1 = X(1,:); X = bsxfun(@minus,X,X1);

opt_val = Inf;
% for each interval width...
for m = round(n*(max_width:-step_sizes(2):min_width))
    % scale and bin the data in the intervals
    nbins = round(3*log2(1+m/2));
    H = bsxfun(@times,X(1:m,:),nbins./X(m,:));
    logq = log(histc(H,[0:nbins-1,Inf]) + 0.01);

    % for each shape value...
    for b=1:length(beta)
        bounds = zbounds{b};
        % evaluate truncated generalized Gaussian pdf at bin centers
        x = bounds(1)+(0.5:(nbins-0.5))/nbins*diff(bounds);
        p = exp(-abs(x).^beta(b))*rescale(b); p=p'/sum(p);

        % calc KL divergences
        kl = sum(bsxfun(@times,p,bsxfun(@minus,log(p),logq(1:end-1,:)))) + log(m);

        % update optimal parameters
        [min_val,idx] = min(kl);
        if min_val < opt_val
            opt_val = min_val;
            opt_beta = beta(b);
            opt_bounds = bounds;
            opt_lu = [X1(idx) X1(idx)+X(m,idx)];
        end
    end
end

% recover distribution parameters at optimum
alpha = (opt_lu(2)-opt_lu(1))/diff(opt_bounds);
mu = opt_lu(1)-opt_bounds(1)*alpha;
beta = opt_beta;

% calculate the distribution's standard deviation from alpha and beta
sig = sqrt((alpha^2)*gamma(3/beta)/gamma(1/beta));


function test_bug1082

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_prepare_vol_sens ft_prepare_headmodel ft_compute_leadfield ft_plot_topo3d ft_headmodel_openmeeg

fprintf('***  DIAGNOSTICAL INFORMATION ***\n');
fprintf('test script is running on host: %s\n', gethostname());

%%

% generate a unit sphere
[pnt, tri] = mesh_sphere(162);

% create the BEM geometries (in mm)
bnd = [];
bnd.pnt = pnt * 100;
bnd.tri = tri; % normals outwards
% bnd.tri = fliplr(tri); % normals inwards

% create a set of electrodes
sel = find(pnt(:,3)>0);
sens = [];
sens.chanpos = pnt(sel,:) * 100;
sens.elecpos = sens.chanpos;
for i=1:length(sel)
  sens.label{i} = sprintf('chan%03d', i);
end

% this is the position of the dipole
pos = [0 0 50];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% calculate volume conductors

tmpcfg = [];
tmpcfg.conductivity = [1];
tmpcfg.method = 'singlesphere';
vol1 = ft_prepare_headmodel(tmpcfg,bnd);
[vol1, sens1] = ft_prepare_vol_sens(vol1, sens);
lf1 = ft_compute_leadfield(pos, sens1, vol1);

tmpcfg = [];
tmpcfg.conductivity = [1];
tmpcfg.method = 'openmeeg';
vol2 = ft_prepare_headmodel(tmpcfg,bnd);
[vol2, sens2] = ft_prepare_vol_sens(vol2, sens);
lf2 = ft_compute_leadfield(pos, sens2, vol2);

tmpcfg = [];
tmpcfg.conductivity = [1];
tmpcfg.method = 'dipoli';
vol3 = ft_prepare_headmodel(tmpcfg,bnd);
[vol3, sens3] = ft_prepare_vol_sens(vol3, sens);
lf3 = ft_compute_leadfield(pos, sens3, vol3);

%%

figure;
subplot(2,2,1); ft_plot_topo3d(sens1.chanpos, lf1(:,1))
subplot(2,2,2); ft_plot_topo3d(sens1.chanpos, lf1(:,2))
subplot(2,2,3); ft_plot_topo3d(sens1.chanpos, lf1(:,3))
colorbar

figure;
subplot(2,2,1); ft_plot_topo3d(sens2.chanpos, lf2(:,1))
subplot(2,2,2); ft_plot_topo3d(sens2.chanpos, lf2(:,2))
subplot(2,2,3); ft_plot_topo3d(sens2.chanpos, lf2(:,3))
colorbar

figure;
subplot(2,2,1); ft_plot_topo3d(sens3.chanpos, lf3(:,1))
subplot(2,2,2); ft_plot_topo3d(sens3.chanpos, lf3(:,2))
subplot(2,2,3); ft_plot_topo3d(sens3.chanpos, lf3(:,3))
colorbar

% figure;
% subplot(2,2,1); ft_plot_topo3d(senst.chanpos, lft(:,1))
% subplot(2,2,2); ft_plot_topo3d(senst.chanpos, lft(:,2))
% subplot(2,2,3); ft_plot_topo3d(senst.chanpos, lft(:,3))
% colorbar


function residual ( )

%*****************************************************************************80
%
%% RESIDUAL assembles the residual vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 April 2006
%
  global area
  global beta
  global difeta
  global fzero
  global indx
  global kappa
  global lambda1
  global lambda2
  global m
  global nel
  global neqn
  global nnodes
  global node
  global nq
  global nu1
  global nu2
  global nunk
  global xc
  global xl
  global xr

  resid = zeros(nunk*neqn,1 );
  xq = zeros(3);
  wq = zeros(3);
%
%  Set up quadrature information on [-1,1]
%
  [ xq, wq ] = gauss3pt;

  for it = 1 : nel

    for iq = 1 : nq

      x = xc(node(it,1)) + area(it) / 2.0 * ( xq(iq) + 1.0 );
      ar = area(it) * wq(iq);
%
%  Evaluate solutions at quadrature points.
%
      [ vh, vhx ] = eval_pt ( x, it, vcur );
      [ ch, chx ] = eval_pt ( x, it, ccur );
      [ fh, fhx ] = eval_pt ( x, it, fcur );
      [ etah, etahx ] = eval_pt ( x, it, etacur );
      [ vhold, vhxold ] = eval_pt ( x, it, vold );
      [ chold, chxold ] = eval_pt ( x, it, cold );
      [ fhold, fhxold ] = eval_pt ( x, it, fold );
      [ etahold, etahxold ] = eval_pt ( x, it, etaold );

      tauxovertau = tau_prob ( fh, fhx, ch, chx );

      for nuk = 1 : 4

        for in = 1 : nnodes

          ip = node(it,in);
          i = indx(ip);

          if ( 0 < i )

            [ bb, bx ] = quadbf ( x, it, in, xc, node );

            term_linear = 0;
            term_nonlin = 0;

            if ( nuk == 1 )
              term_time = ( vh - vhold ) / dt;
              term_nonlin = ( lambda1 * vh * etah ) / ( 1.0 + nu1 * vh );
            elseif ( nuk == 2 )
              term_time = ( ch - chold ) / dt;
              term_nonlin = -lambda1 * vh * etah / ( 1.0 + nu1 * vh );
            elseif ( nuk == 3 )
              term_time = ( fh - fhold ) / dt;
              term_nonlin = -beta * fh * ( fzero - fh ) * etah + ...
                lambda2 * ch * fh / ( 1.0 + nu2 * ch);
            elseif ( nuk == 4 )
              term_time = ( etah - etahold ) / dt;
              term_linear = difeta * bx * etah * ( etahx / etah - tauxovertau );
            end

            k = i + ( nuk - 1 ) * nunk;
            [rhs_source] = rhsfun_source ( x, time, bb, nuk );

            resid(k) = resid(k) + ( rhs_source  ...
              - ( term_time + term_nonlin ) * bb - term_linear ) * ar;

          end

        end

      end

    end

  end
%
%  Calculate norm of residual vector.
%
  resid_norm = sqrt ( resid' * resid ) / ( neqn * nunk )

  return
end


function [fPerMc, res2] = calc_GoodnessFit(mCatalog, fMc, fBinning)
% function [fPerMc, res2] = calc_FitComp(mCatalog, fMc, fBinning);
% -----------------------------------------------------------------------------
%
% Function to calculate  Goodness of fit percentage of a fitted to observed
% magnitude frequency distribution
%
% Incoming variables:
% mCatalog
% fMc
% fBinning
%
% Outgoing variables:
% fPerMc : Percentage of fit
% res2   : Result
%
% Author: J. Woessner
% woessner@seismo.ifg.ethz.ch
% last update: 23.01.03

vSel = mCatalog(:,6) > (fMc - (fBinning/2));
nNumberEvents = length(mCatalog(vSel,6));
if nNumberEvents >= 25
    [fDummy fBValue fDummy,  fDummy] =  bmemag(mCatalog(vSel,:));

    fStartMag = fMc; % Starting magnitude (hypothetical Mc)

    % log10(N)=A-B*M
    vMag = [fStartMag:fBinning:10]; % Ending magnitude must be sufficiently high
    vNumber = 10.^(log10(nNumberEvents)-fBValue*(vMag - fStartMag));
    vNumber = round(vNumber);

    % Find the last bin with an event
    nLastEventBin = min(find(vNumber == 0)) - 1;
    if isempty(nLastEventBin)
        nLastEventBin = length(vNumber);
    end
    % Determine set of all magnitude bins with number of events > 0
    ct = round((vMag(nLastEventBin)-fStartMag)*(1/fBinning) + 1);

    PM=vMag(1:ct);
    vNumber = vNumber(1:ct);
    [bval, vDummy] = hist(mCatalog(vSel,6),PM);
    b3 = fliplr(cumsum(fliplr(bval)));    % N for M >= (counted backwards)
    res2 = sum(abs(b3 - vNumber))/sum(b3)*100;
    fPerMc = 100-res2;
end


function [dlnZ_dmu, dlnZ_dvs] = heavisideNoiseGradVals(noise, mu, varsigma, y)

% HEAVISIDENOISEGRADVALS Gradient wrt mu and varsigma of log-likelihood for heaviside noise model.

% IVM

D = size(mu, 2);
c = y./sqrt(varsigma);
u = zeros(size(c));
dlnZ_dmu = zeros(size(c));
for i = 1:D
  u(:, i) = c(:, i).*(mu(:, i) + noise.bias(i));
  dlnZ_dmu(:, i) = c(:, i).*ngaussian(u(:, i))...
            ./(cumGaussian(u(:, i))+ ...
               noise.eta/(1-2*noise.eta));
end
dlnZ_dvs = -.5*c.*u.*dlnZ_dmu;


%-------------------------------------------------------------------------------------------------------------
% This is an implementation of the MCWNNM algorithm for real color image denoising.
% Author:  Jun Xu, csjunxu@comp.polyu.edu.hk
%              The Hong Kong Polytechnic University
%
% Please refer to the following paper if you use this code:
%
% @article{MCWNNM,
% 	author = {Jun Xu and Lei Zhang and David Zhang and Xiangchu Feng},
% 	title = {Multi-channel Weighted Nuclear Norm Minimization for Real Color Image Denoising},
% 	journal = {ICCV},
% 	year = {2017}
% }
%
% Please see the file License.txt for the license governing this code.
%-------------------------------------------------------------------------------------------------------------

clear;
Original_image_dir  =    'kodak_color/';
fpath = fullfile(Original_image_dir, '*.png');
im_dir  = dir(fpath);
im_num = length(im_dir);

nSig = [5 30 15];

Par.nSig      =   nSig;                                 % Variance of the noise image
Par.win =   20;                                   % Non-local patch searching window
Par.Constant         =  2 * sqrt(2);                              % Constant num for the weight vector
Par.Innerloop =   2;                                    % InnerLoop Num of between re-blockmatching
Par.ps       =   6;                            % Patch size
Par.step        =   5;
Par.Iter          =   6;                            % total iter numbers
Par.display = true;

% Par.method = 'WNNM_ADMM'
Par.method = 'MCWNNM_ADMM'
Par.maxIter = 10;

Par.model = '2';

Par.delta     =   0.1;                                  % Parameter between each iter
Par.lambda = 0.75;
Par.mu = 1.001;
Par.rho = 0.05;
% record all the results in each iteration
Par.PSNR = zeros(Par.Iter, im_num, 'single');
Par.SSIM = zeros(Par.Iter, im_num, 'single');
for i = 1:im_num
    Par.image = i;
    Par.nSig0 = nSig;
    Par.nlsp        =   70;   % Initial Non-local Patch number
    Par.I =  double( imread(fullfile(Original_image_dir, im_dir(i).name)) );
    S = regexp(im_dir(i).name, '\.', 'split');
    [h, w, ch] = size(Par.I);
    Par.nim = zeros(size(Par.I));
    for c = 1:ch
        randn('seed',0);
        Par.nim(:, :, c) = Par.I(:, :, c) + Par.nSig0(c) * randn(size(Par.I(:, :, c)));
    end
    fprintf('%s :\n',im_dir(i).name);
    PSNR =   csnr( Par.nim, Par.I, 0, 0 );
    SSIM      =  cal_ssim( Par.nim, Par.I, 0, 0 );
    fprintf('The initial value of PSNR = %2.4f, SSIM = %2.4f \n', PSNR,SSIM);
    %
    time0 = clock;
    if Par.model == '1'
        [im_out, Par] = MCWNNM_ADMM1_Denoising( Par.nim, Par.I, Par );
    elseif Par.model == '2'
        [im_out, Par] = MCWNNM_ADMM2_Denoising( Par.nim, Par.I, Par );
    else
        [im_out, Par] = MCWNNM_ADMM_Denoising( Par.nim, Par.I, Par );
    end
    fprintf('Total elapsed time = %f s\n', (etime(clock,time0)) );
    im_out(im_out>255)=255;
    im_out(im_out<0)=0;
    % calculate the PSNR
    Par.PSNR(Par.Iter, Par.image)  =   csnr( im_out, Par.I, 0, 0 );
    Par.SSIM(Par.Iter, Par.image)      =  cal_ssim( im_out, Par.I, 0, 0 );
    imname = sprintf([Par.method '_nSig' num2str(nSig(1)) num2str(nSig(2)) num2str(nSig(3)) '_' Par.model '_Oite' num2str(Par.Iter) '_Iite' num2str(Par.maxIter) '_rho' num2str(Par.rho) '_mu' num2str(Par.mu) '_lambda' num2str(Par.lambda) '_' im_dir(i).name]);
    imwrite(im_out/255, imname);
    fprintf('%s : PSNR = %2.4f, SSIM = %2.4f \n',im_dir(i).name, Par.PSNR(Par.Iter, Par.image),Par.SSIM(Par.Iter, Par.image)     );
end
mPSNR=mean(Par.PSNR,2);
[~, idx] = max(mPSNR);
PSNR =Par.PSNR(idx,:);
SSIM = Par.SSIM(idx,:);
mSSIM=mean(SSIM,2);
fprintf('The best PSNR result is at %d iteration. \n',idx);
fprintf('The average PSNR = %2.4f, SSIM = %2.4f. \n', mPSNR(idx),mSSIM);
name = sprintf([Par.method '_' Par.model '_nSig' num2str(nSig(1)) num2str(nSig(2)) num2str(nSig(3)) '_' Par.model '_Oite' num2str(Par.Iter) '_Iite' num2str(Par.maxIter) '_rho' num2str(Par.rho) '_mu' num2str(Par.mu) '_lambda' num2str(Par.lambda) '.mat']);
save(name,'nSig','PSNR','SSIM','mPSNR','mSSIM');


%MDL_IR140 Create model of the ABB IRB 140 manipulator
%
% MDL_IRB140_MOD is a script that creates the workspace variable irb140 which
% describes the kinematic characteristics of an ABB IRB 140 manipulator
% using modified DH conventions.
%
% Also define the workspace vectors:
%   qz         zero joint angle configuration
%
% Reference::
% - ABB IRB 140 data sheet
% - "The modeling of a six degree-of-freedom industrial robot for
%   the purpose of efficient path planning",
%   Master of Science Thesis, Penn State U, May 2009,
%   Tyler Carter
%
% See also mdl_irb140, mdl_puma560, mdl_stanford, mdl_twolink, SerialLink.
%
% Notes::
% - SI units of metres are used.
% - The tool frame is in the centre of the tool flange.
% - Zero angle configuration has the upper arm vertical and lower arm
%   horizontal.

% MODEL: ABB, IRB140, 6DOF, modified_DH

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

clear L

% joint angle limits from
% A combined optimization method for solving the inverse kinematics problem...
% Wang & Chen
% IEEE Trans. RA 7(4) 1991 pp 489-
L(1) = Revolute('d', 0.352, 'a', 0, 'alpha', 0, 'offset', 0, 'modified');
L(2) = Revolute('d', 0, 'a', 0.070, 'alpha', pi/2, 'offset', 0, 'modified');
L(3) = Revolute('d', 0, 'a', 0.360, 'alpha', 0, 'offset', 0, 'modified');
L(4) = Revolute('d', 0.380, 'a', 0, 'alpha', pi/2, 'offset', 0, 'modified');
L(5) = Revolute('d', 0, 'a', 0, 'alpha', -pi/2, 'offset', 0, 'modified');
L(6) = Revolute('d', 0, 'a', 0, 'alpha', pi/2, 'offset', 0, 'modified');

L(1).m = 34655.36e-3;
L(1).r = [27.87 43.12 -89.03]*1e-3;
L(1).I = [
    512052539.74 1361335.88 51305020.72
    1361335.88 464074688.59 70335556.04
    51305020.72 70335556.04 462745526.12]*1e-9;

L(2).m = 15994.59e-3;
L(2).r = [ 198.29 9.73 92.43]*1e03;
L(2).I = [
    94817914.40 -3859712.77 37932017.01
    -3859712.77 328604163.24 -1088970.86
    37932017.01 -1088970.86 277463004.88]*1e-9;

L(3).m = 20862.05e-3;
L(3).r = [ -4.56 -79.96 -5.86];
L(3).I = [
    500060915.95 -1863252.17 934875.78
    -1863252.17 75152670.69 -15204130.09
    934875.78 -15204130.09 515424754.34]*1e-9;

irb140 = SerialLink(L, 'name', 'IRB 140', ...
    'manufacturer', 'ABB', 'comment', 'modified DH');

%
% some useful poses
%
qz = [0 0 0 0 0 0]; % zero angles, L shaped pose

clear L


function out = WL_dwtcoeff(y,wname,level)
% WL_dwtcoeff   Discrete wavelet transform coefficients.
%
% Decomposes the time series using a given wavelet and outputs statistics on the
% coefficients obtained up to a maximum level, level.
%
%---INPUTS:
%
% y, the input time series
%
% wname, the mother wavelet, e.g., 'db3', 'sym2' (see Wavelet Toolbox
%           Documentation)
%
% level, the level of wavelet decomposition (can be set to 'max' for the maximum
%               level determined by wmaxlev)

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
%% Check that a Wavelet Toolbox license is available:
% ------------------------------------------------------------------------------
BF_CheckToolbox('wavelet_toolbox')

% ------------------------------------------------------------------------------
%% Check Inputs
% ------------------------------------------------------------------------------
doPlot = false; % Plot results to figures
N = length(y); % Length of the time series

if nargin < 2 || isempty(wname)
    wname = 'db3'; % Daubechies wavelet filter
end
if nargin < 3 || isempty(level)
    level = 3; % level of wavelet decomposition
end
if strcmp(level,'max')
    level = wmaxlev(N,wname);
end

maxLevelAllowed = wmaxlev(N,wname);
if maxLevelAllowed < level
    fprintf(1,'Chosen level is too large for this wavelet on this signal...\n');
end

% ------------------------------------------------------------------------------
%% Perform Wavelet Decomposition
% ------------------------------------------------------------------------------
% Computes the following:
%   (*) Wavelet decomposition vector, c
%   (*) Bookkeeping vector, l

if maxLevelAllowed < level
    [c, l] = wavedec(y, maxLevelAllowed, wname);
else
    [c, l] = wavedec(y, level, wname);
end

%% Expand DWT coefficients for visualization
% nbcol = 64; % color discretization steps
%
% cfd = zeros(level,N); % detail coefficients
% for k = 1:level
%     d = detcoef(c,l,k);
%     d = d(:)';
%     d = d(ones(1,2^k),:);
%     cfd(k,:) = wkeep1(d(:)',N);
% end
%
% cfd =  cfd(:);
% I = find(abs(cfd)<sqrt(eps));
% cfd(I) = zeros(size(I));
% cfd = reshape(cfd,level,N);
% cfd = wcodemat(cfd,nbcol,'row');

%-------------------------------------------------------------------------------
%% Plotting
%-------------------------------------------------------------------------------
if doPlot
    figure('color','w'); box('on');
    colormap(pink(nbcol));
    image(cfd);
    tics = 1:level;
    labs = int2str((1:level)');
    set(gca,'YTicklabelMode','manual','Ydir','normal', 'Box','On','Ytick',tics,'YTickLabel',labs);
    title('Discrete Wavelet Transform, Absolute Coefficients.');
    xlabel('Time (or Space)')
    ylabel('Level');
end

% ------------------------------------------------------------------------------
%% Get statistics on coefficients
% ------------------------------------------------------------------------------
for k = 1:level
    if k <= maxLevelAllowed
        d = detcoef(c,l,k); % detail coefficients at level k
        % maximum coefficient at this level:
        out.(sprintf('maxd_l%u',k)) = max(d);
        % minimum coefficient at this level:
        out.(sprintf('mind_l%u',k)) = min(d);
        % std coefficients at this level:
        out.(sprintf('stdd_l%u',k)) = std(d);
        % 1-D noise coefficient estimate (estimate of the noise std):
        out.(sprintf('noisestd_l%u',k)) = wnoisest(c,l,k);
    else
        out.(sprintf('maxd_l%u',k)) = NaN;
        out.(sprintf('mind_l%u',k)) = NaN;
        out.(sprintf('stdd_l%u',k)) = NaN;
        out.(sprintf('noisestd_l%u',k)) = NaN;
    end
end

% %% Compress Signal
% % Set approximation coefficients to zero
% % nc = wthcoef('a',c,l);
% NC = wthcoef('t',c,l,N,T,SORH);
%
%
% %% Single Level Reconstruction
% X = waverec(c,l,wname);
% plot(X);
% keyboard

%% Extract Approximation Coefficients from wavelet decomposition structure

% CA = appcoef(C,L,wname,level);

end


function G = greens_function_mono(x,y,z,xs,src,f,conf)
%GREENS_FUNCTION_MONO Green's function in the frequency domain
%
%   Usage: G = greens_function_mono(x,y,z,xs,src,f,conf)
%
%   Input options:
%       x,y,z   - x,y,z points for which the Green's function should be
%                 calculated / m
%       xs      - position of the source
%       src     - source model of the Green's function. Valid models are:
%                   'ps'  - point source
%                   'ls'  - line source
%                   'pw'  - plane wave
%                   'dps' - dipole point source
%       f       - frequency of the source / Hz
%       conf    - configuration struct (see SFS_config)
%
%   Output parameters:
%       G       - Green's function evaluated at the points x,y,z
%
%   GREENS_FUNCTION_MONO(x,y,z,xs,src,f,conf) calculates the Green's function
%   for the given source model located at xs for the given points x,y and the
%   frequency f.
%
%   See also: sound_field_mono

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

%% ===== Checking of input  parameters ==================================
% Disabled checking for performance reasons

%% ===== Configuration ==================================================
c = conf.c;
phase = conf.phase;

%% ===== Computation =====================================================
% Frequency
omega = 2*pi*f;
% Calculate Green's function for the given source model
if strcmp('ps',src)
    % Source model for a point source: 3D Green's function.
    %
    %              1  e^(-i w/c |x-xs|)
    % G(x-xs,w) = --- -----------------
    %             4pi      |x-xs|
    %
    % https://sfs.rtfd.io/en/3.2/sources/#equation-fd-point
    %
    G = 1/(4*pi) * exp(-1i*omega/c .* sqrt((x-xs(1)).^2+(y-xs(2)).^2+(z-xs(3)).^2)) ./ ...
            sqrt((x-xs(1)).^2+(y-xs(2)).^2+(z-xs(3)).^2);

elseif strcmp('dps',src)
    % Source model for a dipole point source: derivative of 3D Green's function.
    %
    %  d                1   / iw       1    \   (x-xs) ns
    % ---- G(x-xs,w) = --- | ----- + ------- | ----------- e^(-i w/c |x-xs|)
    % d ns             4pi  \  c     |x-xs| /   |x-xs|^2
    %
    % r = |x-xs|
    r = sqrt((x-xs(1)).^2+(y-xs(2)).^2+(z-xs(3)).^2);
    % scalar = (x-xs) nxs
    scalar = xs(4).*(x-xs(1)) + xs(5).*(y-xs(2))  + xs(6).*(z-xs(3));
    %
    G = 1/(4*pi) .* (1i*omega/c + 1./r) .* scalar./r.^2 .* exp(-1i*omega/c.*r);

elseif strcmp('ls',src)
    % Source model for a line source: 2D Green's function.
    %
    %                i   (2) / w        \
    % G(x-xs,w) =  - -  H0  |  - |x-xs|  |
    %                4       \ c        /
    %
    % https://sfs.rtfd.io/en/3.2/sources/#equation-fd-line
    %
    G = -1i/4 * besselh(0,2,omega/c* ...
        sqrt( (x-xs(1)).^2 + (y-xs(2)).^2 + (z-xs(3)).^2 ));

elseif strcmp('pw',src)
    % Source model for a plane wave:
    %
    % G(x,w) = e^(-i w/c n x)
    %
    % https://sfs.rtfd.io/en/3.2/sources/#equation-fd-plane
    %
    % Direction of plane wave
    nxs = xs(:,1:3) / norm(xs(:,1:3));
    % Calculate sound field
    G = exp(-1i*omega/c.*(nxs(1).*x+nxs(2).*y+nxs(3).*z));
else
    error('%s: %s is not a valid source model for the Green''s function', ...
        upper(mfilename),src);
end

% Add phase to be able to simulate different time steps
G = G .* exp(-1i*phase);