function example2 ( )

%*****************************************************************************80
%
%% EXAMPLE2 demonstrates the use of PDEPE on a scalar PDE.
%
%  Discussion:
%
%    Solve the convection-diffusion equation.
%
%    PDE:
%      ut + (a(x)*u)x = uxx
%    BC:
%      u(t,-oo) = 0, u(t,+oo) = 0
%    IC:
%      u(0,x) = 1 / (1+(x-5)^2)
%
%    Here, the quantity a(x) is a bit complicated.  It is defined by
%      a(x) = 3 (ubar(x))^2 + 2 * ubar(x)
%    where ubar is defined implicitly by the nonlinear equation
%      1/ubar + log ( abs ( (1-ubar)/ubar ) ) = x
%
%    Ubar(x) is the equilibrium solution to the conservation law
%      ut + (u^3-u^2)x = uxx.
%
%    Although the mathematical problem has boundary conditions at infinity,
%    we approximate this by the interval [-50,+50].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2013
%
%  Author:
%
%    Original formulation by P Howard, 2005.
%    This version by John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'EXAMPLE2:\n' );
  fprintf ( 1, '  The convection-diffusion equation.\n' );
  fprintf ( 1, '  ut + (a(x)*u)x = uxx\n' );
  fprintf ( 1, '  u(t,-oo) = 0, u(t,+oo) = 0\n' );
  fprintf ( 1, '  u(0,x) = 1 / (1+(x-5)^2)\n' );
%
%  M defines the coordinate system:
%  0: cartesian
%  1: cylindrical
%  2: spherical
%
  m = 0;
%
%  Define the spatial mesh.
%
  nx = 201;
  xmesh = linspace ( -50.0, +50.0, nx );
%
%  Define the time mesh.
%
  nt = 101;
  tspan = linspace ( 0.0, 10.0, nt );
%
%  Call PDEPE() for the solution.
%
  sol = pdepe ( m, @pdefun, @icfun, @bcfun, xmesh, tspan );
%
%  Even though our solution is "really" a 2D array, PDEPE stores it
%  in a 3D array SOL(:,:,:).  The surf() command needs a 2D array to plot,
%  so let's copy U out of SOL.
%
  u = sol(:,:,1);

  figure ( 1 )
  surf ( xmesh, tspan, u, 'EdgeColor', 'None' );
  title ( 'Example 2: Solution Over Time', 'Fontsize', 16 );
  xlabel ( '<--- X --->' )
  ylabel ( '<--- T --->' );
  zlabel ( '<---U(X,T)--->' );
  filename = 'example2.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved solution plot in file "%s"\n', filename );
%
%  Plot the initial condition, U at time 0.
%
  figure ( 2 )
  plot ( xmesh, u(1,:), 'LineWidth', 3 );
  grid on
  title ( 'Example 2: Initial Condition', 'Fontsize', 16 );
  xlabel ( '<--- X --->' )
  ylabel ( '<--- U(X,T0) --->' );
  filename = 'example2_ic.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Saved initial condition plot in file "%s"\n', filename );
%
%  Plot the solution U at a fixed point, with time varying.
%
  figure ( 3 )
  mid = 1 + floor ( 55 * ( nx - 1 ) / 100 );
  plot ( tspan, u(:,mid), 'LineWidth', 3 );
  grid on
  title ( 'Example 2: Time evolution of solution at X=5.0', 'Fontsize', 16 );
  xlabel ( '<--- T --->' )
  ylabel ( '<--- U(5.0,T) --->' );
  filename = 'example2_profile.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Saved time evolution plot in file "%s"\n', filename );
%
%  Animate the profile.
%  I wish I could also display the running value of time, but it
%  does not seem possible.
%
  figure ( 4 )
  fig = plot ( xmesh, u(1,:), 'erase', 'xor' );
  title ( 'Profile animation', 'Fontsize', 16 );
  grid on
  for k = 2 : length ( tspan )
    set ( fig, 'xdata', xmesh, 'ydata', u(k,:) );
    pause ( 0.4 );
  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'EXAMPLE2:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function value = degwave ( x )

%*****************************************************************************80
%
%% DEGWAVE determines the value of UBAR in the equation.
%
%  Discussion:
%
%    MATLAB's zero finder "fzero()" must be called in order to determine
%    the value of UBAR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the spatial location.
%
%    Output, real VALUE, the value of A(X).
%
  if ( x < -35.0 )
    value = 1.0;
  elseif ( 2.0 < x )
    guess = 1.0 / x;
    value = fzero ( @f, guess, [], x );
  elseif ( -2.5 < x )
    guess = 0.6;
    value = fzero ( @f, guess, [], x );
  else
    guess = 1.0 - exp ( - 2.0 ) * exp ( x );
    value = fzero ( @f, guess, [], x );
  end

  return
end
function value = f ( u, x )

%*****************************************************************************80
%
%% F evaluates a function, which should be zero if U = UBAR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real U, the estimated solution value at X.
%
%    Input, real X, the spatial location.
%
%    Output, real VALUE, the function value.
%
  value = ( 1.0 / u ) + log ( ( 1.0 - u ) / u ) - x;

  return
end
function [ c, f, s ] = pdefun ( x, t, u, dudx )

%*****************************************************************************80
%
%% PDEFUN defines the components of the PDE.
%
%  Discussion:
%
%    The PDE has the form:
%
%      c * du/dt = x^(-m) d/dx ( x^m f ) + s
%
%    where m is 0, 1 or 2,
%    c, f and s are functions of x, t, u, and dudx,
%    and most typically, f = dudx.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the spatial location.
%
%    Input, real T, the current time.
%
%    Input, real U(:,1), the estimated solution at T and X.
%
%    Input, real DUDX(:,1), the estimated spatial derivative of U at T and X.
%
%    Output, real C(:,1), the coefficients of du/dt.
%
%    Output, real F(:,1), the flux terms.
%
%    Output, real S(:,1), the source terms.
%
  c = 1.0;
  ubar = degwave ( x );
  f = dudx - ( 3.0 * ubar.^2 - 2.0 * ubar ) * u;
  s = 0.0;

  return
end
function u0 = icfun ( x )

%*****************************************************************************80
%
%% ICFUN defines the initial conditions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the spatial location.
%
%    Output, real U0(:,1), the value of the solution at the initial time,
%    and location X.
%
  u0 = 1.0 / ( 1.0 + ( x - 5.0 ).^2 );

  return
end
function [ pl, ql, pr, qr ] = bcfun ( xl, ul, xr, ur, t )

%*****************************************************************************80
%
%% BCFUN defines the boundary conditions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real XL, the spatial coordinate of the left boundary.
%
%    Input, real UL(:,1), the solution estimate at the left boundary.
%
%    Input, real XR, the spatial coordinate of the right boundary.
%
%    Input, real UR(:,1), the solution estimate at the right boundary.
%
%    Output, real PL(:,1), the Dirichlet portion of the left boundary condition.
%
%    Output, real QL(:,1), the coefficient of the flux portion of the left
%    boundary condition.
%
%    Output, real PR(:,1), the Dirichlet portion of the right boundary condition.
%
%    Output, real QR(:,1), the coefficient of the flux portion of the right
%    boundary condition.
%
  pl = ul;
  ql = 0.0;
  pr = ur;
  qr = 0.0;

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


function i = mytsearch ( x, y, t, xi, yi, i )

%*****************************************************************************80
%
%% MYTSEARCH: Find the enclosing triangle for points in a 2D plane.
%
%  Discussion:
%
%    The indices of the triangles enclosing the points in [XI,YI] are
%    returned.  The triangulation T of [X,Y] must be convex.  Points lying
%    outside the triangulation are assigned a NaN index.
%
%    I is an optional initial guess for the indices. A full search is
%    done using the standard TSEARCH function for points with an invalid
%    initial guess.
%
%  Author:
%
%    Darren Engwirda
%.

% I/O and error checks
if nargin<6
   i = [];
   if nargin<5
      error('Wrong number of inputs');
   end
elseif nargin>6
   error('Wrong number of inputs');
end
if nargout>1
   error('Wrong number of outputs');
end
ni = size(xi,1);
if (numel(xi)~=ni) || (numel(yi)~=ni) || ...
      (numel(x)~=numel(y)) || (~isempty(i) && (numel(i)~=ni))
   error('Wrong input dimensions');
end

% Translate to the origin and scale the min xy range onto [-1,1]
% This is absolutely critical to avoid precision issues for large problems!
maxxy = max([x,y]);
minxy = min([x,y]);
den = 0.5*min(maxxy-minxy);

x  = ( x-0.5*(minxy(1)+maxxy(1))) / den;
y  = ( y-0.5*(minxy(2)+maxxy(2))) / den;
xi = (xi-0.5*(minxy(1)+maxxy(1))) / den;
yi = (yi-0.5*(minxy(2)+maxxy(2))) / den;

% Check initial guess
if ( ~isempty(i) )
   k = find(i>0 & ~isnan(i));

   tri = i(k);

   n1 = t(tri,1);
   n2 = t(tri,2);
   n3 = t(tri,3);

   ok = sameside(x(n1),y(n1),x(n2),y(n2),xi(k),yi(k),x(n3),y(n3)) & ...
        sameside(x(n2),y(n2),x(n3),y(n3),xi(k),yi(k),x(n1),y(n1)) & ...
        sameside(x(n3),y(n3),x(n1),y(n1),xi(k),yi(k),x(n2),y(n2));

   j = true(ni,1);
   j(k(ok)) = false;
else
   j = true(ni,1);
end
%
% Do a full search for points that failed
%
if any(j)
   i(j) = tsearch(x,y,t,xi(j),yi(j));
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function i = sameside(xa,ya,xb,yb,x1,y1,x2,y2)

% Test if [x1(i),y1(i)] and [x2(i),y2(i)] lie on the same side of the line
% AB(i).

  dx = xb-xa;
  dy = yb-ya;
  a1 = (x1-xa).*dy-(y1-ya).*dx;
  a2 = (x2-xa).*dy-(y2-ya).*dx;

% If sign(a1)=sign(a2) the points lie on the same side
  i = false(length(xa),1);
  i(a1.*a2>=0.0) = true;

end


function [ u, l ] = vand2_inverse_ul ( n, x )

%*****************************************************************************80
%
%% VAND2_INVERSE_UL returns the UL factors of the VAND2 inverse matrix.
%
%  Discussion:
%
%    inverse ( A ) = U * L
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 November 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Richard Turner,
%    Inverse of the Vandermonde Matrix with Applications,
%    NASA Technical Note TN D-3547, 1966.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real X(N), the values that define A.
%
%    Output, real U(N,N), L(N,N), the UL factors of inverse(A).
%
  u = zeros ( n, n );

  for i = 1 : n
    u(i,i) = 1.0;
    if ( i == 1 )
      for j = i + 1 : n
        u(i,j) = - u(i,j-1) * x(j-1);
      end
    else
      for j = i + 1 : n
        u(i,j) = u(i-1,j-1) - u(i,j-1) * x(j-1);
      end
    end
  end

  l = zeros ( n, n );

  for i = 1 : n
    l(i,1:i) = 1.0;
    for j = 1 : i
      for k = 1 : i
        if ( j ~= k )
          l(i,j) = l(i,j) / ( x(j) - x(k) );
        end
      end
    end
  end

  return
end


function elli = fitEllipse(varargin)
%FITELLIPSE Fit an ellipse to a set of 2D points.
%
%   ELLI = fitEllipse(PTS)
%
%   Example
%     elli = [50 40  30 10 20];
%     pts = ellipseToPolygon(elli, 60) + randn(60,2);
%     figure; hold on; drawPoint(pts, 'ko');
%     axis equal; axis([0 100 0 100]);
%     ellFit = fitEllipse(pts);
%     drawEllipse(ellFit, 'b')
%
%   This is a rewrite of an original function from the authors cited below.
%   Changes from original submission include:
%   * convert angle of result ellipse to degrees (to comply with MatGeom
%       convention)
%   * update comments
%
% Authors:
% Andrew Fitzgibbon, Maurizio Pilu, Bob Fisher
% Reference: "Direct Least Squares Fitting of Ellipses", IEEE T-PAMI, 1999
%
% https://fr.mathworks.com/matlabcentral/fileexchange/3215-fit_ellipse
%
%  @Article{Fitzgibbon99,
%   author = "Fitzgibbon, A.~W.and Pilu, M. and Fisher, R.~B.",
%   title = "Direct least-squares fitting of ellipses",
%   journal = pami,
%   year = 1999,
%   volume = 21,
%   number = 5,
%   month = may,
%   pages = "476--480"
%  }
%
%   See also
%     geom2d, ellipses2d, createEllipse, equivalentEllipse
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2022-07-16, using Matlab 9.12.0.1884302 (R2022a)
% Copyright 2022 INRAE - BIA Research Unit - BIBS Platform (Nantes)

%% Process input arguments

if nargin==1
    var = varargin{1};
    X = var(:,1);
    Y = var(:,2);
else
    X = varargin{1};
    Y = varargin{2};
end

%% Normalize data

% recenter and reduce range
mx = mean(X);
my = mean(Y);
sx = (max(X) - min(X)) / 2;
sy = (max(Y) - min(Y)) / 2;
x = (X-mx) / sx;
y = (Y-my) / sy;

% Force to column vectors
x = x(:);
y = y(:);

%% Main processing

% Build design matrix
D = [ x.*x  x.*y  y.*y  x  y  ones(size(x)) ];

% Build scatter matrix
S = D' * D;

% Build 6x6 constraint matrix
C(6,6) = 0;
C(1,3) = -2;
C(2,2) = 1;
C(3,1) = -2;

% Solve eigensystem

% Break into blocks
tmpA = S(1:3,1:3);
tmpB = S(1:3,4:6);
tmpC = S(4:6,4:6);
tmpD = C(1:3,1:3);
tmpE = tmpC \ tmpB';

[evec_x, eval_x] = eig(tmpD \ (tmpA - tmpB*tmpE));

% Find the positive (as det(tmpD) < 0) eigenvalue
I = real(diag(eval_x)) < 1e-8 & ~isinf(diag(eval_x));

% Extract eigenvector corresponding to negative eigenvalue
A = real(evec_x(:,I));

% Recover the bottom half...
evec_y = -tmpE * A;
A = [A; evec_y];

% re-calibrate
par = [
    A(1)*sy*sy,   ...
    A(2)*sx*sy,   ...
    A(3)*sx*sx,   ...
    -2*A(1)*sy*sy*mx - A(2)*sx*sy*my + A(4)*sx*sy*sy,   ...
    -A(2)*sx*sy*mx - 2*A(3)*sx*sx*my + A(5)*sx*sx*sy,   ...
    A(1)*sy*sy*mx*mx + A(2)*sx*sy*mx*my + A(3)*sx*sx*my*my   ...
    - A(4)*sx*sy*sy*mx - A(5)*sx*sx*sy*my   ...
    + A(6)*sx*sx*sy*sy   ...
    ]';

%% Identify parameters

% rotation angle
theta = 0.5 * atan2(par(2), par(1) - par(3));

% pre-comptute trigonometrics
cost = cos(theta);
sint = sin(theta);
sin2 = sint .* sint;
cos2 = cost .* cost;
cos_sin = sint .* cost;

%
Ao =  par(6);
Au =  par(4) .* cost + par(5) .* sint;
Av = -par(4) .* sint + par(5) .* cost;
Auu = par(1) .* cos2 + par(3) .* sin2 + par(2) .* cos_sin;
Avv = par(1) .* sin2 + par(3) .* cos2 - par(2) .* cos_sin;

% ROTATED = [Ao Au Av Auu Avv]

tuCentre = - Au./(2.*Auu);
tvCentre = - Av./(2.*Avv);
wCentre = Ao - Auu.*tuCentre.*tuCentre - Avv.*tvCentre.*tvCentre;

uCentre = tuCentre .* cost - tvCentre .* sint;
vCentre = tuCentre .* sint + tvCentre .* cost;

Ru = -wCentre ./ Auu;
Rv = -wCentre ./ Avv;

Ru = sqrt(abs(Ru)) .* sign(Ru);
Rv = sqrt(abs(Rv)) .* sign(Rv);

elli = [uCentre, vCentre, Ru, Rv, rad2deg(theta)];


function alpha = db2neper(alpha, y)
%DB2NEPER   Convert decibels to nepers.
%
% DESCRIPTION:
%       db2neper converts an attenuation coefficient in units of
%       dB/(MHz^y cm) to units of Nepers/((rad/s)^y m).
%
% USAGE:
%       alpha = db2neper(alpha)
%       alpha = db2neper(alpha, y)
%
% INPUTS:
%       alpha       - attenuation in dB/(MHz^y cm)
%
% OPTIONAL INPUTS:
%       y           - power law exponent (default = 1)
%
% OUTPUTS:
%       alpha       - attenuation in Nepers/((rad/s)^y m)
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 27th March 2009
%       last update - 5th 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 neper2db

% 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/>.

if nargin == 1
    y = 1;
end
alpha = 100*alpha.*(1e-6/(2*pi)).^y./(20*log10(exp(1)));


function surfacedata = quarticssurface

surfacedata = struct('phi',@phi,'gradient', @gradient, 'unitoutnormal',...
    @unitoutnormal, 'project', @project, 'initmesh',@initmesh,...
    'meancurvature', @meancurvature, 'u', @u, 'f', @f, 'gra_u',@gra_u);

function [node, elem] = initmesh
M = load('meshdata/quartics4528.mat');
node = M.node;
elem = M.elem;
end

function z = phi(p)
% level set function
x = p(:,1); y = p(:,2); z = p(:,3);
z = (x.^2 - 1).^2 + (y.^2 - 1).^2 + (z.^2 - 1).^2 - 1.05;
end

function n = unitoutnormal(p)
%
n = gradient(p);
l = sqrt(sum(n.^2,2));
n = n./repmat(l,1,3);
end

function n = gradient(p)
% gradient of phi
x = p(:,1); y = p(:,2); z = p(:,3);
n = [4*(x.^2 - 1).*x,4*(y.^2 - 1).*y, 4*(z.^2 - 1).*z];
end

function [node,dist] = project(p)
% projection function

s = sign(phi(p));

node = p;

normalAtNode = gradient(node);
valueAtNode = phi(node);
node = node - valueAtNode*ones(1,3).*normalAtNode./(dot(normalAtNode,normalAtNode,2)*ones(1,3));

vector = (-s)*ones(1,3).*(node - p);

d = s.*sqrt(dot(vector,vector,2));

normalAtNode = gradient(node);

node = p - d*ones(1,3).*normalAtNode./(sqrt(dot(normalAtNode,normalAtNode,2))*ones(1,3));

valueAtNode = phi(node);

normalAtNode = gradient(node);

vector = (-s)*ones(1,3).*(node - p);

d = s.*sqrt(dot(vector,vector,2));

e1 = normalAtNode./(sqrt(dot(normalAtNode, normalAtNode,2))*ones(1,3))-vector./(sqrt(dot(vector,vector,2))*ones(1,3));
error=sqrt(valueAtNode.^2./(dot(normalAtNode, normalAtNode,2))+dot(e1,e1,2));

k=1;
while max(abs(error)) > 1e-6 && k<200

    k=k+1;

    node = node - valueAtNode*ones(1,3).*normalAtNode./(dot(normalAtNode,normalAtNode,2)*ones(1,3));

    vector = -s*ones(1,3).*(node - p);
    d = s.*sqrt(dot(vector,vector,2));
    normalAtNode = gradient(node);

    node = p - d*ones(1,3).*normalAtNode./(sqrt(dot(normalAtNode,normalAtNode,2))*ones(1,3));

    valueAtNode = phi(node);

    normalAtNode = gradient(node);

    vector = (-s)*ones(1,3).*(node - p);

    d = s.*sqrt(dot(vector,vector,2));
    e1 = normalAtNode./(sqrt(dot(normalAtNode, normalAtNode,2))*ones(1,3))-vector./(sqrt(dot(vector,vector,2))*ones(1,3));
    error=sqrt(valueAtNode.^2./(dot(normalAtNode, normalAtNode,2))+dot(e1,e1,2));
end

if nargout == 2
    dist = d;
end
end

function mc = meancurvature(p)
x = p(:,1); y = p(:,2); z = p(:,3);
s = (x.^2-1).^2.*x.^2 + (y.^2-1).^2.*y.^2 + (z.^2-1).^2.*z.^2;
s1 =(3 .* z .^ 2 - 2 + 3 .* y .^ 2) .* x .^ 6 + (-6 .* y .^ 2 - 6 .* z .^ 2 + 4) .* x .^ 4 ...
    + (-2 + 6 .* y .^ 2 + 3 .* z .^ 6 - 6 .* z .^ 4 - 6 .* y .^ 4 + 6 .* z .^ 2 + 3 .* y .^ 6) .* x .^ 2 ...
+ (3 .* z .^ 2 - 2) .* y .^ 6 + (4 - 6 .* z .^ 2) .* y .^ 4 + ...
    (-2 + 6 .* z .^ 2 + 3 .* z .^ 6 - 6 .* z .^ 4) .* y .^ 2 - 2 .* z .^ 6 - 2 .* z .^ 2 + 4 .* z .^ 4;

mc = 0.5 * s1 ./ (s.^(3/2));
end

function z = f(p)
p = project(p);
x = p(:,1); y = p(:,2); z = p(:,3);
x2 = x.^2; y2 = y.^2; z2 = z.^2;
x4 = x2.^2; y4 = y2.^2; z4 = z2.^2;

x31 = x2.*x - x;
y31 = y2.*y - y;
z31 = z2.*z - z;

d1 = x31.^2 + y31.^2 + z31.^2;
d2 = x31 + y31 + z31;
t = exp(x+y+z);

d3 = (z2 + y2 - 2/3).*x4.*(x2 - 2);
d4 = (z4.*(z2-2) + 2*z2 + y4.*(y2 -2) + 2*y2 -2/3).*x2;
d5 = (z2 - 2/3).*y4.*(y2 - 2);
d6 = (z4.*(z2 -2) + 2*z2 - 2/3).*y2;

z = -2*t.*((-(y31+z31).*x31 - z31.*y31 + d1).*d1 -1.5*(d3 + d4 + d5 + d6 - 2/3*z31.^2).*d2)./d1.^2;
end

function z = u(p)
% exact solution
p = project(p);
z = exp(sum(p,2));
end

function z = gra_u(p)

p = project(p);
x = p(:,1); y = p(:,2); z = p(:,3);
x2 = x.^2; y2 = y.^2; z2 = z.^2;

x31 = x2.*x - x;
y31 = y2.*y - y;
z31 = z2.*z - z;

d1 = x31.^2 + y31.^2 + z31.^2;
t = exp(x+y+z);
z1 = (y31+z31).*x31 - y31.^2 - z31.^2;
z2 = (x31+z31).*y31 - x31.^2 - z31.^2;
z3 = (x31+y31).*z31 - x31.^2 - y31.^2;

z = -(t./d1)*ones(1,3).*[z1,z2,z3];
end

end


function J = warp_jacobian(nx, ny, warp, transform)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%J = WARP_JACOBIAN(NX, NY, WARP, TRANSFORM)
% This function computes the jacobian J of warp transform with respect
% to parameters. In case of homography/euclidean transform, the jacobian depends on
% the parameter values, while in affine/translation case is totally invariant.
%
% Input variables:
% NX:           the x-coordinate values of the horizontal side of ROI (i.e. [xmin:xmax]),
% NY:           the y-coordinate values of vertical side of ROI (i.e. [ymin:ymax]),
% WARP:         the warp transform (used only in homography and euclidean case),
% TRANSFORM:    the type of adopted transform
% {'affine''homography','translation','euclidean'}
%
% Output:
% J:            The jacobian matrix J
%--------------------------------------
% $ Ver: 1.3, 13/5/2012,  released by Georgios D. Evangelidis, INRIA, FRANCE
% Email: georgios.evangelidis@inria.fr
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

snx=length(nx);
sny=length(ny);

Jx=nx(ones(1,sny),:);
Jy=ny(ones(1,snx),:)';
J0=0*Jx;
J1=J0+1;

switch lower(transform)
    case 'homography'

    xy=[Jx(:)';Jy(:)';ones(1,snx*sny)];

    %3x3 matrix transformation
    A = warp;
    A(3,3) = 1;

    % new coordinates
    xy_prime = A * xy;

    % division due to homogeneous coordinates
    xy_prime(1,:) = xy_prime(1,:)./xy_prime(3,:);
    xy_prime(2,:) = xy_prime(2,:)./xy_prime(3,:);

    den = xy_prime(3,:)';

    Jx(:) = Jx(:) ./ den;
    Jy(:) = Jy(:) ./ den;
    J1(:) = J1(:) ./ den;

    Jxx_prime = Jx;
    Jxx_prime(:) = Jxx_prime(:) .* xy_prime(1,:)';
    Jyx_prime = Jy;
    Jyx_prime(:) = Jyx_prime(:) .* xy_prime(1,:)';

    Jxy_prime = Jx;
    Jxy_prime(:) = Jxy_prime(:) .* xy_prime(2,:)';
    Jyy_prime = Jy;
    Jyy_prime(:) = Jyy_prime(:) .* xy_prime(2,:)';

    J = [Jx, J0, -Jxx_prime, Jy, J0, - Jyx_prime, J1, J0;...
        J0, Jx, -Jxy_prime, J0, Jy, -Jyy_prime, J0, J1];

        case 'affine'

    J = [Jx, J0, Jy, J0, J1, J0;...
        J0, Jx, J0, Jy, J0, J1];
    case 'translation'

    J = [J1, J0;...
        J0, J1];

    case 'euclidean'

        mycos = warp(1,1);
        mysin = warp(2,1);

        Jx_prime = -mysin*Jx - mycos*Jy;
        Jy_prime =  mycos*Jx - mysin*Jy;

        J = [Jx_prime, J1, J0;...
            Jy_prime, J0, J1];

    otherwise
        error('function WARP_JACOBIAN: Unknown transform!');
end


function v = std3(f)
%STD3   Standard deviation of a CHEBFUN3.
%   V = STD3(F) computes the standard deviation of the CHEBFUN3 object F,
%   i.e.,
%             STD3(F)^2 = 1 / V*sum3(|F(x,y,z) - M|^2)
%
%   where V is the volume of the domain of F and M is the mean of F.
%
% See also CHEBFUN3/MEAN, CHEBFUN3/MEAN2, CHEBFUN3/MEAN3, CHEBFUN3/STD and
% CHEBFUN3/STD2.

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

h = f - mean3(f);
v = sqrt(mean3(h .* conj(h)));

end


function [xmin,fmin]=Trust_Region_Method(x0)

eta1=0.01;
eta2=0.75;
gamma1=0.5;
gamma2=2.0;
Delta_bar=3;
Delta0=1;
epsilon=1e-6;
k=0;
xk=x0;
Deltak=Delta0;

while 1
    % stop criteria
    gk=gradient(xk);
    Bk=Hessian(xk);
    if norm(gk)<=epsilon
        xmin=xk;
        fmin=objective_fun(xmin);
        k
        return;
    end

    % solve subproblem
    % Cauchy displacement
    if gk'*Bk*gk<=0
        dk=-(Deltak/norm(gk))*gk;
    else
        dk=-min(norm(gk)^2/(gk'*Bk*gk),Deltak/norm(gk))*gk;
    end

    % Dogleg method
%     dku=-((norm(gk)^2)/(gk'*Bk*gk))*gk;
%     dkN=-inv(Bk)*gk;
%     if norm(dku)>=Deltak
%         dk=-(Deltak/norm(gk))*gk;
%     elseif norm(dku)<Deltak && norm(dkN)<=Deltak
%         dk=dkN;
%     else
%         temp1=inf;
%         for tau=0:0.01:1
%             if norm((1-tau)*dku+tau*dkN)-Deltak<temp1
%                 dk=(1-tau)*dku+tau*dkN;
%             end
%         end
%     end

    % ratio
    rhok=(objective_fun(xk)-objective_fun(xk+dk))/(mk(zeros(length(xk),1),xk)-mk(dk,xk));

    % adjust trust region radius
    if rhok<=eta1
        Deltak=gamma1*Deltak;
    elseif rhok>=eta2 && (norm(dk)<=Deltak+epsilon && norm(dk)>=Deltak-epsilon)
        Deltak=min(gamma2*Deltak,Delta_bar);
    end

    % update iteration point
    if rhok>eta1
        xk=xk+dk;
    end
    k=k+1;
end


function [ p2, dp2, p1 ] = legendre_recur ( x, n )

%*****************************************************************************80
%
%% LEGENDRE_RECUR finds the value and derivative of a Legendre polynomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the point at which polynomials are evaluated.
%
%    Input, integer N, the order of the polynomial to be computed.
%
%    Output, real P2, the value of P(N)(X).
%
%    Output, real DP2, the value of P'(N)(X).
%
%    Output, real P1, the value of P(N-1)(X).
%
  p1 = 1.0;
  dp1 = 0.0;

  p2 = x;
  dp2 = 1.0;

  for i = 2 : n

    p0 = p1;
    dp0 = dp1;

    p1 = p2;
    dp1 = dp2;

    p2 = ( ( 2 * i - 1 ) * x * p1 ...
         + (   - i + 1 )     * p0 ) ...
         / (     i     );

    dp2 = ( ( 2 * i - 1 ) * ( p1 + x * dp1 ) ...
          - (     i - 1 ) * dp0 ) ...
          / (     i     );

  end

  return
end


function variance = coupon_variance ( j, n )

%*****************************************************************************80
%
%% COUPON_VARIANCE returns the variance of the Coupon PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer J, the number of distinct coupons to be collected.
%    J must be between 1 and N.
%
%    Input, integer N, the number of distinct coupons.
%
%    Output, real VARIANCE, the variance of the number of
%    coupons that must be collected in order to just get J distinct kinds.
%
  if ( n < j )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'COUPON_VARIANCE - Fatal error!\n' );
    fprintf ( 1, '  Number of distinct coupons desired must be no more\n' );
    fprintf ( 1, '  than the total number of distinct coupons.\n' );
    error ( 'COUPON_VARIANCE - Fatal error!' );
  end

  variance = 0.0;
  for i = 1 : j
    variance = variance + ( i - 1 ) / ( n - i + 1 )^2;
  end
  variance = variance * n;

  return
end


function x = spgridcc(levelseq, d)
% SPGRIDCC   Computes the Clenshaw-Curtis sparse grid points in
% each coordinate direction.
%    X = SPGRIDCC(LEVELSEQ, D)  Computes the sparse grid points for
%    the given sequence of index sets LEVELSEQ and dimension D. The
%    coordinate value of dimension i is stored in column i of the
%    matrix X. One row of matrix X represents one grid point.

% Author : Andreas Klimke
% Version: 1.1
% Date   : April 20, 2004

% Change log:
% V1.0   : July 30, 2003
%          Initial version
% V1.1   : April 20, 2004
%          Altered function header and simplified code.

% Get the number of levels
nlevels = size(levelseq,1);

% Compute number of points
totalpoints = spseqdimcc(levelseq,d);

% index contains the index of the resulting array containing all
% subdomains of the level.
index = 1;

x = zeros(totalpoints,d);

for kl = 1:nlevels
	level = levelseq(kl,:);
	for i = 1:d
		% compute the points, scaled to [0,1]
		if level(i) == 0
			% take the interval bounds if the level is zero
			c = 0.5;
		elseif level(i) == 1
			c = [0; 1];
		else
			c = ( ( ( (1:2^(level(i)-1)) *2 ) -1).*2^(-level(i)))';
		end
		% Compute the number of grid points per dimension, store it
		% in repvec.
		repvec = ones(1,d);
		for k = 1:d
			if level(k) == 0
				% repvec(k) = 1;
			elseif level(k) < 3
				repvec(k) = 2;
			else
				repvec(k) = 2^(level(k)-1);
			end
		end
		npoints = prod(repvec);
		repvec(i) = 1;
		c = repmat(shiftdim(c, 1-i), repvec);
		x(index:index+npoints-1,i) = c(:);
	end
	index = index + npoints;
end

% -----------------------------------------------------------------
function n = spseqdimcc(levelseq, d)
% SPSEQDIMCC   Compute number of grid points for given sequence of
% index sets.

% Author : Andreas Klimke
% Version: 1.0
% Date   : April 20, 2004

n = 0;
for k = 1:size(levelseq,1);
	ntemp = 1;
	for l = 1:d
		lev = levelseq(k,l);
		if lev == 0
			continue;
		elseif lev <= 2
			ntemp = ntemp * 2;
		else
			ntemp = ntemp * 2^(lev-1);
		end
	end
	n = n + ntemp;
end


function op = proj_l2group( q,group_indices )
%PROJ_L2GROUP   Projection onto the intersection of scaled 2-norm balls.
%    OP = PROJ_L2GROUP( Q, GROUP_INDICES ) returns an operator implementing the
%    indicator function for the intersection of 2-norm ball of size q_k,
%    i.e. intersection_k B_k
%    where B_k = { X |  norm( X(indK) ) <= q(k) }
%    and indK is indexed by group_indices. Group_indices keeps track
%    of the last index in each index set.
%    The index sets must be non-overlapping.
%    For example, if K=2, and ind1 = 1:10 and ind2 = 11:20,
%    then group_indices = [10,20].
%    Q must be a vector of length K or a vector of length N.
%
% With contributions from Joseph Salmon
%
% See also: proj_l2.m

if isempty(q), q = 1; end
% Make sure q is a column vector
if size(q,1) == 1 && size(q,2) > 1, q = q.'; end
q = expand_q(q,group_indices);
op = @(varargin)proj_l2_q( q,group_indices, varargin{:} );
end

function [ v, x ] = proj_l2_q( q,group_indices, x, t )
v = 0;
nrm=group_norm(x,group_indices);

switch nargin,
	case 3,
		if nargout == 2,
			error( 'This function is not differentiable.' );
		elseif norm( nrm, 'inf') > q,
			v = Inf;
		end
	case 4,
            x = x .* min(1,( q ./ nrm ));
	otherwise,
		error( 'Not enough arguments.' );
end
end

function y=group_norm(x,group_indices)
% the groups are indexed by their end point
    K=length(group_indices);
    y=zeros(size(x));
    j=1; % start of the group
    for k=1:K
        end_point=group_indices(k);
        y(j:end_point)=norm(x(j:end_point));
        j=end_point+1;
    end
end

function Q = expand_q(q,group_indices)
% If we have 2 groups, each of size 10
% (group_indices = [10,20]), then q should
% be a vector of size 20. But it's more convenient
% for the user to pass in a vector of size 2, so this function
% will convert it...
n = group_indices(end);
K = length(group_indices);
if length(q) < n
    Q = ones(n,1);
    j=1; % start of the group
    for k=1:K
        end_point=group_indices(k);
        Q(j:end_point) = q(k);
        j=end_point+1;
    end
else
    Q = q;
end
end

% 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.


%The Gabor filter is basically a Gaussian (with variances sx and sy along x and y-axes respectively)
%modulated by a complex sinusoid (with centre frequencies U and V along x and y-axes respectively)
%described by the following equation
%%
%               1                -1     x  ^    y  ^
%%% G(x,y) = ---------- * exp ([----{(----) 2+(----) 2}+2*pi*i*(Ux+Vy)])
%            2*pi*sx*sy           2    sx       sy

%% But we use one dimentional gabor filter as below

%              1              -1    x  ^
%%% G(x) = ---------- * exp ([----(----) 2+2*pi*i*Ux])
%           2*pi*sx            2    sx

%And

%              1              -1    y  ^
%%% G(y) = ---------- * exp ([----(----) 2+2*pi*i*Uy])
%           2*pi*sy            2    sy
% By using these filters,reduces the filtering operations from O(M2N2) to O(MN2).

%The output is
%%% Iu(x,y) = Conv(I(x,y),g(y));
%%% Iv(x,y) = Conv(I(x,y),g(x));
%%%Ifilt = sqrt(Iu^2+Iv^2);
%% Where I is input image.

%% Describtion :

%% I : Input image
%% Sx & Sy : Variances along x and y-axes respectively
%% U & V : Centre frequencies  along x and y-axes respectively

%% Gx & Gy : The output filters as described above
%% gabout : The output filtered image

%%  Author : Ahmad poursaberi  e-mail : a.poursaberi@ece.ut.ac.ir
%%          Faulty of Engineering, Electrical&Computer Department,Tehran
%%          University,Iran,June 2004

function [Gx,Gy,gabout] = gaborfilter1D(I,Sx,Sy,U,V);

if isa(I,'double')~=1
    I = double(I);
end
warning off
for x = -fix(Sx):fix(Sx)
    for y = -fix(Sy):fix(Sy)
        Gx(fix(Sx)+x+1) = (1/(2*pi*Sx))*exp(-.5*((x/Sx)^2)+2*pi*i*U*x);
        Gy(fix(Sy)+y+1) = (1/(2*pi*Sy))*exp(-.5*((y/Sy)^2)+2*pi*i*V*y);

    end
end

gaboutu = conv2(I,double(Gy),'same');
gaboutv = conv2(I,double(Gx),'same');
gabout = uint8(sqrt(gaboutu.*gaboutu + gaboutv.*gaboutv));


% HOMOGRAPHY2D - computes 2D homography
%
% Usage:   H = homography2d(x1, x2)
%          H = homography2d(x)
%
% Arguments:
%          x1  - 3xN set of homogeneous points
%          x2  - 3xN set of homogeneous points such that x1<->x2
%
%           x  - If a single argument is supplied it is assumed that it
%                is in the form x = [x1; x2]
% Returns:
%          H - the 3x3 homography such that x2 = H*x1
%
% This code follows the normalised direct linear transformation
% algorithm given by Hartley and Zisserman "Multiple View Geometry in
% Computer Vision" p92.

% Copyright (c) 2003-2005 Peter Kovesi
% School of Computer Science & Software Engineering
% The University of Western Australia
% pk at csse uwa edu au
% http://www.csse.uwa.edu.au/~pk
%
% 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, 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.

% May 2003  - Original version.
% Feb 2004  - Single argument allowed for to enable use with RANSAC.
% Feb 2005  - SVD changed to 'Economy' decomposition (thanks to Paul O'Leary)

function H = homography2d(varargin)

    [x1, x2] = checkargs(varargin(:));

    % Attempt to normalise each set of points so that the origin
    % is at centroid and mean distance from origin is sqrt(2).
    [x1, T1] = normalise2dpts(x1);
    [x2, T2] = normalise2dpts(x2);

    % Note that it may have not been possible to normalise
    % the points if one was at infinity so the following does not
    % assume that scale parameter w = 1.

    Npts = length(x1);
    A = zeros(3*Npts,9);

    O = [0 0 0];
    for n = 1:Npts
	X = x1(:,n)';
	x = x2(1,n); y = x2(2,n); w = x2(3,n);
	A(3*n-2,:) = [  O  -w*X  y*X];
	A(3*n-1,:) = [ w*X   O  -x*X];
	A(3*n  ,:) = [-y*X  x*X   O ];
    end

    [U,D,V] = svd(A,0); % 'Economy' decomposition for speed

    % Extract homography
    H = reshape(V(:,9),3,3)';

    % Denormalise
    H = T2\H*T1;

%--------------------------------------------------------------------------
% Function to check argument values and set defaults

function [x1, x2] = checkargs(arg);

    if length(arg) == 2
	x1 = arg{1};
	x2 = arg{2};
	if ~all(size(x1)==size(x2))
	    error('x1 and x2 must have the same size');
	elseif size(x1,1) ~= 3
	    error('x1 and x2 must be 3xN');
	end

    elseif length(arg) == 1
	if size(arg{1},1) ~= 6
	    error('Single argument x must be 6xN');
	else
	    x1 = arg{1}(1:3,:);
	    x2 = arg{1}(4:6,:);
	end
    else
	error('Wrong number of arguments supplied');
    end


% the following script calculates and plots
% the skin friction factors for smooth pipes for
% laminar, turbulent and transitonal flows
% for Reynnolds number up to 100000
%  CALLED FUNCTION: ffsmooth
% ---------------------------------------------------------------
% The MATLAB function was created by Tibor Balint, December 1998
% TBoreal Research Corporation, Toronto, Ont. Canada
% (tibor@netcom.ca) and also, University of Warwick, UK
% ---------------------------------------------------------------

clear
format long g;                  % set the format of the calculations
n=2000; %number of points to be plotted

RE=linspace(600,100000,n);

for i=1:n
   ffsm(i)=ffsmooth(RE(i)); %calculate the friction factors
end

figure(1)
h=get(0,'CurrentFigure');
set(h,'Name','Friction Factor for Smooth Pipes','NumberTitle','off');
orient landscape;
loglog(RE,ffsm)
%plot(RE,ffsm)
set(findobj('Type','line'),'Color','k','LineWidth',1.2);  %set all the lines black, and the line width to 1.2 from 0.5 (default)
grid on;
axis ([600 10^5 0.015 0.1]);
%axis tight;
xlabel('{\bf Reynolds number} {\it ( Re={UD_h}/\nu )}');
ylabel('{\bfFriction factor} {\it f}');
title ('{\bf FRICTION FACTOR FOR SMOOTH PIPES}','FontSize',13);
set(gca,'ytick',[0.015 0.02 0.025 0.03 0.04 0.05 0.06 0.07 0.08 0.09 0.1]);
text(1.05*10^7,0.032,'\it0.006','FontSize',7);
text(1000,0.0205,'{\it To be used for laminar, transitional and turbulent flows,}','FontSize',8);
text(1000,0.0195,'{\it  in smooth pipes at Reynolds numbers below 100000}','FontSize',8);
text(0.99*10^3,0.06,'{\it Laminar flow}','FontSize',8,'Rotation',-62);
text(1.05*10^4,0.0295,'{\it Turbulent flow}','FontSize',8,'Rotation',-28);
text(2400,0.031,'{\it Flow Transition}','FontSize',8,'Rotation',50);
text(4*10^4,0.013,'MATLAB script by Tibor Balint, April 1998','FontSize',6);
return
% -------------- end of the script ----------------------


function [gt,relres,iter]=gabfirtight(Lsupport,g,a,M,varargin)
%GABFIRTIGHT  Compute FIR tight window
%   Usage: gt=gabfirtight(Lsupport,g,a,M);
%          gt=gabfirtight(Lsupport,g,a,M, varagin);
%
%   Input parameters:
%     Lsupport  : Length of the tight window
%     g      : Initial window function
%     a      : Time shift
%     M      : Number of Channels
%
%   Output parameters:
%     gt     : Tight window
%
%   `gabfirtight(Lsupport,g,a,M)` computes an FIR window *gd* which is tight.
%   The FIR dual window will be supported on *Lsupport* samples.
%
%   This function solve a convex optimization problem that can be written
%   as:
%
%   .. gd  = argmin_x    || alpha x||_1 +  || beta Fx||_1
%
%   ..                 + || omega (x -g_l) ||_2^2 + delta || x ||_S0
%
%   ..                 + gamma || nabla F x ||_2^2 + mu || nabla x ||_2^2
%
%   ..     such that  x is a tight FIR windows
%
%   .. math:: \begin{split}  \text{gd}  = & \text{arg} \min_x   \| \alpha x \|_1 +  \| \beta \mathcal{F}x\|_1  \\ & + \| \omega (x - g_l) \|_2^2  \\ & \delta \| x \|_{S0}+ \mu \| \nabla x \|_2^2 +\gamma \| \nabla \mathcal{F} x \|_2^2 \\ &    \text{such that } x \text{ is a tight FIR window} \end{split}
%
%   **Note**: This function require the unlocbox. You can download it at
%   `<http://unlocbox.sourceforge.net>`_
%
%   The function uses an iterative algorithm to compute the approximate
%   FIR tight windows. Warning The algorithm solve a non convex problem and
%   might be stack in bad local minima. The algorithm can be controlled by
%   the following flags:
%
%     'alpha',alpha  Weight in time. If it is a scalar, it represent the
%                  weights of the entire L1 function in time. If it is a
%                  vector, it is the associated weight assotiated to each
%                  component of the L1 norm (length: Ldual).
%                  Default value is $\alpha=0$.
%                  **Warning**: this value should not be too big in order to
%                  avoid the the L1 norm proximal operator kill the signal.
%                  No L1-time constraint: $\alpha=0$
%
%     'beta',beta  Weight in frequency. If it is a scalar, it represent the
%                  weights of the entire L1 function in frequency. If it is a
%                  vector, it is the associated weight assotiated to each
%                  component of the L1 norm in frequency. (length: Ldual).
%                  Default value is $\beta=0$.
%                  **Warning**: this value should not be too big in order to
%                  avoid the the L1 norm proximal operator kill the signal.
%                  No L1-frequency constraint: $\beta=0$
%
%     'omega',omega
%                  Weight in time of the L2-norm. If it is a scalar,
%                  it represent the
%                  weights of the entire L2 function in time. If it is a
%                  vector, it is the associated weight assotiated to each
%                  component of the L2 norm (length: Ldual).
%                  Default value is $\omega=0$.
%                  No L2-time constraint: $\omega=0$
%
%     'glike',g_l  $g_l$ is a windows in time. The algorithm try to shape
%                  the dual window like $g_l$. Normalization of $g_l$ is done
%                  automatically. To use option omega should be different
%                  from 0. By default $g_d=0$.
%
%     'mu',mu      Weight of the smooth constraint Default value is 1.
%                  No smooth constraint: $\mu=0$
%
%     'gamma',gamma  Weight of the smooth constraint in frequency.
%                    Default value is 1. No smooth constraint: $\gamma=0$
%
%     'delta',delta  Weight of the S0-norm. Default value is 0.
%                    No S0-norm: $\delta=0$
%
%     'dual'       Look for a dual windows (default)
%
%     'painless'   Construct a starting guess using a painless-case
%                  approximation. This is the default
%
%     'zero'       Choose a starting guess of zero.
%
%     'rand'       Choose a random starting phase.
%
%     'tol',t      Stop if relative residual error is less than the
%                  specified tolerance.
%
%     'maxit',n    Do at most n iterations. default 200
%
%     'print'      Display the progress.
%
%     'debug'      Display all the progresses.
%
%     'quiet'      Don't print anything, this is the default.
%
%     'fast'       Fast algorithm, this is the default.
%
%     'slow'       Safer algorithm, you can try this if the fast algorithm
%                  is not working. Before using this, try to iterate more.
%
%     'printstep',p  If 'print' is specified, then print every p'th
%                    iteration. Default value is p=10;
%
%     'hardconstraint' Force the projection at the end (default)
%
%     'softconstaint' Do not force the projection at the end
%
%   See also: gaboptdual, gabdual, gabtight, gabfirdual, gabconvexopt

% Author: Nathanael Perraudin
% Date  : 18 Feb 2014

if nargin<4
  error('%s: Too few input parameters.',upper(mfilename));
end;

[gt,relres,iter]=gabconvexopt(g,a,M,varargin{:}, 'support',Lsupport,'tight');


%  This m-file calculates the area of a circle,
%  and displays the result.
radius = 2.5;
area = pi * 2.5^2;
string = ['The area of the circle is ' num2str(area)];
disp(string);


function err ( )

%*****************************************************************************80
%
%% ERR computes the L2 and H1 errors using a 2 point Gauss quadrature rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 April 2006
%

%
%  Set up the quadrature information.
%
  [ xq, wq ] = gauss3pt ( );

  el2 = 0.0;
  eh1 = 0.0;

  for it = 1 : nel

    for iq = 1 : nq
      ar = area(it) * wq(iq);
      x = xc(node(it,1)) + area(it) / 2.0 * ( xq(iq) + 1.0 );
      uh = 0.0;
      uhx = 0.0;
      for in = 1 : nnodes
        ip = node(it,in);
        [ bb, bx ] = quadbf ( x, it, in, xc, node );
        i = indx(ip);
        if ( i ~= 0 )
          uh = uh + bb * f(i);
          uhx = uhx + bx * f(i);
        end
      end
      [ uex, uexx ] = exact ( x );
      el2 = el2 + ar * ( uh - uex )^2;
      eh1 = eh1 + ar * ( uhx - uexx )^2 );
    end
  end

  el2 = sqrt ( el2 )
  eh1 = sqrt ( eh1 )

  return
end


function expmat = MatrixLog6(T)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a transformation matrix T in SE(3).
% Returns the corresponding se(3) representation of exponential
% coordinates.
% Example Input:
%
% clear; clc;
% T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
% expmat = MatrixLog6(T)
%
% Output:
% expc6 =
%         0         0         0         0
%         0         0   -1.5708    2.3562
%         0    1.5708         0    2.3562
%         0         0         0         0

[R, p] = TransToRp(T);
omgmat = MatrixLog3(R);
if isequal(omgmat, zeros(3))
    expmat = [zeros(3), T(1: 3, 4); 0, 0, 0, 0];
else
    theta = acos((trace(R) - 1) / 2);
    expmat = [omgmat, (eye(3) - omgmat / 2 ...
                      + (1 / theta - cot(theta / 2) / 2) ...
                        * omgmat * omgmat / theta) * p;
              0, 0, 0, 0];
end
end


function jacobi_test01 ( )

%*****************************************************************************80
%
%% JACOBI_TEST01 tests JACOBI1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'JACOBI_TEST01:\n' );

  it_num = 400;
  n = 20;

  x_exact = ( 1 : n )';
  a = dif2 ( n );
  b = a * x_exact;

  x = zeros ( n, 1 );
  x_plot(1:n,1) = x;

  step = 1 : it_num + 1;
  r = nan ( it_num+1, 1 );
  xm = nan ( it_num+1, 1 );

  r(1,1) = ( norm ( a * x - b ) ).^2;

  for it = 1 : it_num

    x_new = jacobi1 ( n, a, b, x );
%   x_new = jacobi2 ( n, a, b, x );

    r(it+1,1) = ( norm ( a * x_new - b ) ).^2;
    x_plot(1:n,it+1) = x_new;
%
%  Display the residual.
%
    figure ( 1 )
    plot ( step, log ( r ), 'm-*' )
    title ( 'Log (Residual^2)' )
    xlabel ( 'Step' )
    ylabel ( 'Residual' )
    grid
%
%  Display the motion.
%
    xm(it,1) = sum ( ( x_new(:) - x(:) ).^2 ) / n;

    figure ( 2 )
    plot ( step, log ( xm ), 'm-*' )
    title ( 'Log (Average generator motion)' )
    xlabel ( 'Step' )
    ylabel ( 'Motion' )
    grid
%
%  Update the solution
%
    x = x_new;

  end
%
%  Plot the evolution of the locations of the generators.
%
  figure ( 3 )

  y = ( 0 : it_num );
  for k = 1 : n
    plot ( x_plot(k,1:it_num+1), y )
    hold on;
  end
  grid on
  hold off;

  title ( 'Generator evolution.' );
  xlabel ( 'Generator positions' );
  ylabel ( 'Iterations' );

  return
end


function [b_value_ma, a_value_ma, b_error_ma] = ma(mag,cumnum)

    % compute the slope, intercept, and their errors using
    % "Major Axis" regression.
    % the inputs are two column vectors.
    % e.g. [b,a,e] = ma(X,Y) were X and Y are column vectors of the same length.
    % outputs are the b and a values and the standard error.

    report_this_filefun();

    X = [mag cumnum];
    m = mean(X);
    ssx = (X(:,1)-m(1))'*(X(:,1)-m(1));
    ssy = (X(:,2)-m(2))'*(X(:,2)-m(2));
    ssxy = (X(:,1)-m(1))'*(X(:,2)-m(2));

    % MAJOR AXIS REGRESSION
    [V,D] = eig(cov(X));
    % the slope of the line in the direction of max. variance
    b_value_ma = V(2,2)/V(1,2);
    a_value_ma = m(2) - (b_value_ma * m(1));

    % the standard error in the slopes
    r = ssxy/(sqrt(ssx.*ssy));
    b_error_ma = abs(b_value_ma)*sqrt((1-r^2)/length(X));
end


function x = sample_paths_eigen ( n, n2, rhomax, rho0, correlation )

%*****************************************************************************80
%
%% SAMPLE_PATHS_EIGEN: sample paths for stationary correlation functions.
%
%  Discussion:
%
%    This method uses the eigen-decomposition of the correlation matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points on each path.
%
%    Input, integer N2, the number of paths.
%
%    Input, real RHOMAX, the maximum value of RHO.
%
%    Input, real RHO0, the correlation length.
%
%    Input, @CORRELATION, a handle for a correlation function.
%
%    Output, real X(N,N2), the sample paths.
%

%
%  Choose N equally spaced sample points from 0 to RHOMAX.
%
  rho_vec = linspace ( 0.0, rhomax, n );
%
%  Evaluate the correlation function.
%
  cor_vec = correlation ( n, rho_vec, rho0 );

  cor = zeros ( n, n );
%
%  Construct the correlation matrix;
%
%  From the vector
%    [ C(0), C(1), C(2), ... C(N-1) ]
%  construct the vector
%    [ C(N-1), ..., C(2), C(1), C(0), C(1), C(2), ...  C(N-1) ]
%  Every row of the correlation matrix can be constructed by a subvector
%  of this vector.
%
  cor_vec = [ cor_vec(n:-1:2)', cor_vec(1:n)' ];
  for i = 1 : n
    cor(i,1:n) = cor_vec(n+1-i:2*n-i);
  end
%
%  Get the eigendecomposition of COR:
%
%    COR = V * D * V'.
%
%  Because COR is symmetric, V is orthogonal.
%
  [ v, d ] = eig ( cor );
%
%  We assume COR is non-negative definite, and hence that there
%  are no negative eigenvalues.  If this is not the case,
%  warn the user, hope the numbers are only slightly negative,
%  and reset them to 0.
%
  dmin = min ( min ( d ) );

  if ( dmin < - sqrt ( eps ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SAMPLE_PATHS_EIGEN - Warning!\n' );
    fprintf ( 1, '  Negative eigenvalues observed as low as %g\n', dmin );
  end

  d = max ( d, 0.0 );
%
%  Compute the eigenvalues of the factor C.
%
  sqrt_d = sqrt ( d );
%
%  Compute C, such that C' * C = COR.
%
  c = v * sqrt_d * v';
%
%  Compute N independent random normal values.
%
  r(1:n,1:n2) = randn ( n, n2 );
%
%  Get the variables X which have correlation COR.
%
  x(1:n,1:n2) = c(1:n,1:n) * r(1:n,1:n2);

  return
end


%   By WangLin
%   2015-11-5
%   wanglin193@hotmail.com

clear all
%close all
%create a canvas
a = 32;
cc = 255*rand(1,3);
bb = 255*rand(1,3);
for i=1:3
    im(:,:,i) = [   cc(i)*ones(a,a),25+zeros(a,a);
                   25+zeros(a,a),cc(i)*ones(a,a)];
end

im = uint8(repmat(im,4,5));

[imh,imw,imc] = size(im);

%define landmarks
ps = [1,1;imw-0,1;imw-0,imh-0;1,imh-0;
      0.4*imw,imh*3/8;
      0.6*imw,imh*3/8;
      0.4*imw,imh*5/8;
      0.6*imw,imh*5/8];
pd = ps;
%move some points
% pd(5:8,:) = pd(5:8,:) + 15*[1,1;-1,1;1,-1;-1,-1];
pd(1:4,:) = pd(1:4,:) + 30*[1,1;-1,1;1,-1;-1,-1];
%pd(5:8,:) = pd(5:8,:) + 30*[-1,1;-1,-1;1,1;1,-1];

% ps=ps(5:end,:);
% pd=pd(5:end,:);

figure

subplot(2,2,1)
imshow(uint8(im))
title('Orininal image with base landmarks');
hold on
plot( ps(:,1),ps(:,2),'r.' );
 plot( pd(:,1),pd(:,2),'g.' ); % added by Abdo

%% Gaussian RBF function
[imo1,mask1] = rbfwarp2d( im, ps, pd,'gau',imw*0.1 );
[imo2,mask2] = rbfwarp2d( im, ps, pd,'gau',imw*0.5 );

subplot(2,2,2)
imshow(uint8(imo1));
title('RBF warping Rad = 0.1*imwidth');
hold on
plot( ps(:,1),ps(:,2),'r.' );
plot( pd(:,1),pd(:,2),'g.' );

subplot(2,2,3)
imshow(uint8(imo2));
title('RBF warping Rad = 0.5*imwidth');
hold on
plot( ps(:,1),ps(:,2),'r.' );
plot( pd(:,1),pd(:,2),'g.' );

%% Thin plate warping
tic
for i=1:150
[imo1,mask1] = rbfwarp2d( im, ps, pd,'thin');
end
toc
subplot(2,2,4)
imshow(uint8(imo1));
title('Thin-plate warping');
hold on
plot( ps(:,1),ps(:,2),'r.' );
plot( pd(:,1),pd(:,2),'g.' );


function [IDX,sep] = otsu(I,n)

%OTSU Global image thresholding/segmentation using Otsu's method.
%   IDX = OTSU(I,N) segments the image I into N classes by means of Otsu's
%   N-thresholding method. OTSU returns an array IDX containing the cluster
%   indices (from 1 to N) of each point. Zero values are assigned to
%   non-finite (NaN or Inf) pixels.
%
%   IDX = OTSU(I) uses two classes (N=2, default value).
%
%   [IDX,sep] = OTSU(...) also returns the value (sep) of the separability
%   criterion within the range [0 1]. Zero is obtained only with data
%   having less than N values, whereas one (optimal value) is obtained only
%   with N-valued arrays.
%
%   Notes:
%   -----
%   It should be noticed that the thresholds generally become less credible
%   as the number of classes (N) to be separated increases (see Otsu's
%   paper for more details).
%
%   If I is an RGB image, a Karhunen-Loeve transform is first performed on
%   the three R,G,B channels. The segmentation is then carried out on the
%   image component that contains most of the energy.
%
%   Example:
%   -------
%   load clown
%   subplot(221)
%   X = ind2rgb(X,map);
%   imshow(X)
%   title('Original','FontWeight','bold')
%   for n = 2:4
%     IDX = otsu(X,n);
%     subplot(2,2,n)
%     imagesc(IDX), axis image off
%     title(['n = ' int2str(n)],'FontWeight','bold')
%   end
%   colormap(gray)
%
%   Reference:
%   ---------
%   Otsu N, A Threshold Selection Method from Gray-Level Histograms,
%   IEEE Trans. Syst. Man Cybern. 9:62-66;1979
%
%   See also GRAYTHRESH, IM2BW
%
%   -- Damien Garcia -- 2007/08, revised 2010/03
%   Visit my website for more details about OTSU

error(nargchk(1,2,nargin))

% Check if is the input is an RGB image
isRGB = isrgb(I);

assert(isRGB | ndims(I)==2,...
    'The input must be a 2-D array or an RGB image.')

%% Checking n (number of classes)
if nargin==1
    n = 2;
elseif n==1;
    IDX = NaN(size(I));
    sep = 0;
    return
elseif n~=abs(round(n)) || n==0
    error('MATLAB:otsu:WrongNValue',...
        'n must be a strictly positive integer!')
elseif n>255
    n = 255;
    warning('MATLAB:otsu:TooHighN',...
        'n is too high. n value has been changed to 255.')
end

I = single(I);

%% Perform a KLT if isRGB, and keep the component of highest energy
if isRGB
    sizI = size(I);
    I = reshape(I,[],3);
    [V,D] = eig(cov(I));
    [tmp,c] = max(diag(D));
    I = reshape(I*V(:,c),sizI(1:2)); % component with the highest energy
end

%% Convert to 256 levels
I = I-min(I(:));
I = round(I/max(I(:))*255);

%% Probability distribution
unI = sort(unique(I));
nbins = min(length(unI),256);
if nbins==n
    IDX = ones(size(I));
    for i = 1:n, IDX(I==unI(i)) = i; end
    sep = 1;
    return
elseif nbins<n
    IDX = NaN(size(I));
    sep = 0;
    return
elseif nbins<256
    [histo,pixval] = hist(I(:),unI);
else
    [histo,pixval] = hist(I(:),256);
end
P = histo/sum(histo);
clear unI

%% Zeroth- and first-order cumulative moments
w = cumsum(P);
mu = cumsum((1:nbins).*P);

%% Maximal sigmaB^2 and Segmented image
if n==2
    sigma2B =...
        (mu(end)*w(2:end-1)-mu(2:end-1)).^2./w(2:end-1)./(1-w(2:end-1));
    [maxsig,k] = max(sigma2B);

    % segmented image
    IDX = ones(size(I));
    IDX(I>pixval(k+1)) = 2;

    % separability criterion
    sep = maxsig/sum(((1:nbins)-mu(end)).^2.*P);

elseif n==3
    w0 = w;
    w2 = fliplr(cumsum(fliplr(P)));
    [w0,w2] = ndgrid(w0,w2);

    mu0 = mu./w;
    mu2 = fliplr(cumsum(fliplr((1:nbins).*P))./cumsum(fliplr(P)));
    [mu0,mu2] = ndgrid(mu0,mu2);

    w1 = 1-w0-w2;
    w1(w1<=0) = NaN;

    sigma2B =...
        w0.*(mu0-mu(end)).^2 + w2.*(mu2-mu(end)).^2 +...
        (w0.*(mu0-mu(end)) + w2.*(mu2-mu(end))).^2./w1;
    sigma2B(isnan(sigma2B)) = 0; % zeroing if k1 >= k2

    [maxsig,k] = max(sigma2B(:));
    [k1,k2] = ind2sub([nbins nbins],k);

    % segmented image
    IDX = ones(size(I))*3;
    IDX(I<=pixval(k1)) = 1;
    IDX(I>pixval(k1) & I<=pixval(k2)) = 2;

    % separability criterion
    sep = maxsig/sum(((1:nbins)-mu(end)).^2.*P);

else
    k0 = linspace(0,1,n+1); k0 = k0(2:n);
    [k,y] = fminsearch(@sig_func,k0,optimset('TolX',1));
    k = round(k*(nbins-1)+1);

    % segmented image
    IDX = ones(size(I))*n;
    IDX(I<=pixval(k(1))) = 1;
    for i = 1:n-2
        IDX(I>pixval(k(i)) & I<=pixval(k(i+1))) = i+1;
    end

    % separability criterion
    sep = 1-y;

end

IDX(~isfinite(I)) = 0;

%% Function to be minimized if n>=4
    function y = sig_func(k)

        muT = sum((1:nbins).*P);
        sigma2T = sum(((1:nbins)-muT).^2.*P);

        k = round(k*(nbins-1)+1);
        k = sort(k);
        if any(k<1 | k>nbins), y = 1; return, end

        k = [0 k nbins];
        sigma2B = 0;
        for j = 1:n
            wj = sum(P(k(j)+1:k(j+1)));
            if wj==0, y = 1; return, end
            muj = sum((k(j)+1:k(j+1)).*P(k(j)+1:k(j+1)))/wj;
            sigma2B = sigma2B + wj*(muj-muT)^2;
        end
        y = 1-sigma2B/sigma2T; % within the range [0 1]

    end

end

function isRGB = isrgb(A)
% --- Do we have an RGB image?
% RGB images can be only uint8, uint16, single, or double
isRGB = ndims(A)==3 && (isfloat(A) || isa(A,'uint8') || isa(A,'uint16'));
% ---- Adapted from the obsolete function ISRGB ----
if isRGB && isfloat(A)
    % At first, just test a small chunk to get a possible quick negative
    mm = size(A,1);
    nn = size(A,2);
    chunk = A(1:min(mm,10),1:min(nn,10),:);
    isRGB = (min(chunk(:))>=0 && max(chunk(:))<=1);
    % If the chunk is an RGB image, test the whole image
    if isRGB, isRGB = (min(A(:))>=0 && max(A(:))<=1); end
end
end


function [aDeriv,aJacob,aHess,papt]=aCVRuv(x)
%ACVRUV The drift function for a continuous-time motion model where the
%       the target state is given in monostatic range and u-v direction
%       cosine coordinates and the motion is constant-velocity in 3D
%       Cartesian coordinates.
%
%INPUTS: x The 6XN state vector of N targets in the order of
%          [r;u;;v;rDot;uDot;vDot], where r is monostatic range and u and v
%          are direction cosines. The target is assumed to always be in
%          front of the radar and not pass behind the radar.
%
%OUTPUTS: aDeriv The 6XN set of time-derivatives of the N state vectors
%                under the Cartesian linear motion model in r-u-v
%                coordinates.
%         aJacob The 6X6XN set of Jacobians of aDeriv, which can be useful
%                in extended Kalman filters. This is the derivative of each
%                component of aDeriv (selected by row) with respect to the
%                elements of the state [r,u,v,rDot,uDot,vDot] selected by
%                column.
%
%A full derivation of the dynamic model is provided in [1]. Summarizing it
%here, let rVec be a position vector. We shall use the orthonormal basis
%vectors:
%uVec1=[u;v;sqrtw]
%uVec2=[sqrt(w/(1-v2));0;-u/sqrt(1-v2)]
%uVec3=[-u*v/sqrt(1-v2);sqrt(1-v2);-v*sqrt(w/(1-v2))];
%Note that uVec2 is a normalized version of the derivative of uVec1 with
%respect to u and uVec3 is cross(u1,u2). The time-derivatives of the basis
%vectors can themselves be expressed as basis vectors:
%uVec1Dot=c1*u2+c2*u3
%uVec2Dot=c3*u1+c4*c3
%uVec3Dot=c5*u1+c6*u2
%The coefficients c1 through c6 are omitted for brevity but can be found
%by taking the total derivative of each basis vector with respect to time
%and then taking the dot products of the result with the orthonormal basis
%vectors.
%The vector rVec is written
%rVec=r*u1
%The velocity vector is the derivative of rVec. Using the chain rule and
%substituting in the derivative parameters for the basis vectors,
%rVecDot=rDot*uVec1+r*c1*uVec2+r*c2*uVec3
%The decond derivative is more complicated, but simplifies to
%rVecDDot=a1*uVec1+a2*uVec2+a3*uVec3
%where two D's indicates two time-derivatives.
%a1 is a radial acceleration and a3 and a3 are two other Cartesian
%components of acceleration. The expressions for them are
% a1=rDDot+r*(c1*c3+c2*c5);
% a2=2*rDot*c1+r*(c1Dot+c2*c6);
% a3=2*rDot*c2+r*(c2Dot+c1*c4);
%The expression for the drift function comes from setting a1=a2=a3=0 and
%solving for the second derivative terms. The second derivatives in u and v
%arise through the derivatives of c1 and c2.
%
%EXAMPLE:
%Here, we verify that integrating forward with this model is equivalent to
%linear motion in Cartesian coordinates.
% xInitCart=[400;920;1000;-100;20;64];
% T=50;%The prediction time.
% F=FPolyKal(T,6,1);
% xInitRuv=stateCart2Ruv(xInitCart);
% xEndCart=F*xInitCart;
% RelTol=1e-10;
% AbsTol=1e-13;
% xStepsRuv=RKAdaptiveOverRange(xInitRuv,[0;T],@(x,t)aCVRuv(x),0.01,0,[],[],RelTol,AbsTol);
% xEndRuvRK=xStepsRuv(:,end);
% xEndRuvExact=stateCart2Ruv(xEndCart);
% max(abs(xEndRuvRK-xEndRuvExact)./xEndRuvExact)
%One will observe that the error is less than 1e-11, which is a good
%agreement.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic Linear Dynamic Models in Local Coordinates,"
%    Naval Research Laboratory, Washington, D.C., Tech. Rep.
%    NRL/MR/5344--19-9882, 24 Aug. 2019.
%
%August 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

r=x(1,:);
u=x(2,:);
v=x(3,:);
rDot=x(4,:);
uDot=x(5,:);
vDot=x(6,:);

u2=u.^2;
v2=v.^2;
r2=r.^2;

w2=max(0,1-u2-v2);
w=sqrt(w2);

diffV2=1-v2;
diffV=sqrt(diffV2);

diffU2=1-u2;
diffU=sqrt(diffU2);

uDot2=uDot.*uDot;
vDot2=vDot.*vDot;
w3=w2.*w;

rDDot=-(r*(-uDot2*diffV2-2*u*uDot*v*vDot-diffU2*vDot2))/w2;
uDDot=-((2*rDot*uDot)/r)-u*(vDot2*diffU2+uDot2*diffV2+2*u*v*uDot*vDot)/w2;
vDDot=-((2*rDot*vDot)/r)-v*(uDot2*diffV2+vDot2*diffU2+2*u*v*uDot*vDot)/w2;

aDeriv=[rDot;uDot;vDot;rDDot;uDDot;vDDot];

if(nargout>1)
    N=size(x,2);
    if(N>1)
        error('Derivatives are only available for numPoints=1.')
    end

    w4=w2^2;

    ddrDDdr=-(-uDot2*diffV2-2*u*uDot*v*vDot-diffU2*vDot2)/w2;
    ddrDDdu=(2*r*(u*uDot+v*vDot)*(uDot-uDot*v2+u*v*vDot))/w4;
    ddrDDdv=-((2*r*(-u*uDot*v-diffU2*vDot)*(u*uDot+v*vDot))/w4);

    ddrDDduDot=((2*r*(uDot-uDot*v2+u*v*vDot))/w2);
    ddrDDdvDot=-(2*r*(-u*uDot*v-diffU2*vDot))/w2;

    dduDDdr=(2*rDot*uDot)/r2;
    dduDDdu=(-uDot2*(1+u2-v2)*diffV2-4*u*uDot*v*diffV2*vDot+(-1+v2-u2*(-2+u2+3*v2))*vDot2)/w4;
    dduDDdv=(2*u*(-u*uDot*v-diffU2*vDot)*(u*uDot+v*vDot))/w4;
    dduDDdrDot=-((2*uDot)/r);
    dduDDduDot=-((2*rDot)/r)-(2*u*(uDot-uDot*v2+u*v*vDot))/w2;
    dduDDdvDot=-(2*u*(u*uDot*v+vDot-u2*vDot))/w2;

    ddvDDdr=(2*rDot*vDot)/r2;
    ddvDDdu=-((2*v*(u*uDot+v*vDot)*(uDot-uDot*v2+u*v*vDot))/w4);
    ddvDDdv=(uDot2*(u2*(1-3*v2)-diffV2^2)-4*u*diffU2*uDot*v*vDot+diffU2*(-1+u2-v2)*vDot2)/w4;
    ddvDDdrDot=-((2*vDot)/r);
    ddvDDduDot=-(2*v*(uDot-uDot*v2+u*v*vDot))/w2;
    ddvDDdvDot=-((2*rDot)/r)-(2*v*(u*uDot*v+vDot-u2*vDot))/w2;

    aJacob=[0,               0,              0,              1,              0,              0;
            0,               0,              0,              0,              1,              0;
            0,               0,              0,              0,              0,              1;
            ddrDDdr,         ddrDDdu,        ddrDDdv,        0,              ddrDDduDot,     ddrDDdvDot;
            dduDDdr,         dduDDdu,        dduDDdv,        dduDDdrDot,     dduDDduDot,     dduDDdvDot;
            ddvDDdr,         ddvDDdu,        ddvDDdv,        ddvDDdrDot,     ddvDDduDot,     ddvDDdvDot];

    if(nargout>2)
        aHess=zeros(6,6,6);

        u4=u2*u2;
        v4=v2*v2;
        w6=w3*w3;
        diffU4=diffU2*diffU2;
        diffV4=diffV2*diffV2;

        ddrDDdrdr=0;
        ddrDDdudr=(2*(u*uDot+v*vDot)*(uDot-uDot*v2+u*v*vDot))/w4;
        ddrDDdvdr=-((2*(-u*uDot*v-diffU2*vDot)*(u*uDot+v*vDot))/w4);
        ddrDDduDotdr=((2*(uDot-uDot*v2+u*v*vDot))/w2);
        ddrDDdvDotdr=-(-2*u*uDot*v-2*diffU2*vDot)/w2;

        ddrDDdrdu=ddrDDdudr;
        ddrDDdudu=-(2*r*(-uDot2*(1+3*u2-v2)*diffV2-2*u*uDot*v*(3+u2-3*v2)*vDot+v2*(-1-3*u2+v2)*vDot2))/w6;
        ddrDDdvdu=(2*r*(uDot*(1+u2-v2)+2*u*v*vDot)*(2*u*uDot*v+vDot-u2*vDot+v2*vDot))/w6;
        ddrDDduDotdu=(-4*r*u*uDot*(-diffV2)+2*r*v*(1+u2-v2)*vDot)/w4;
        ddrDDdvDotdu=(2*r*v*(uDot*(1+u2-v2)+2*u*v*vDot))/w4;

        ddrDDdrdv=ddrDDdvdr;
        ddrDDdudv=ddrDDdvdu;
        ddrDDdvdv=-(2*r*(u2*uDot2*(-1+u2-3*v2)+2*u*uDot*v*(-3+3*u2-v2)*vDot+diffU2*(-1+u2-3*v2)*vDot2))/w6;
        ddrDDduDotdv=(2*r*u*(2*u*uDot*v+vDot-u2*vDot+v2*vDot))/w4;
        ddrDDdvDotdv=(2*r*u*uDot*(1-u2+v2)+4*r*diffU2*v*vDot)/w4;

        ddrDDdrdRDot=0;
        ddrDDdudRDot=0;
        ddrDDdvdRDot=0;
        ddrDDduDotdRDot=0;
        ddrDDdvDotdRDot=0;

        ddrDDdrduDot=ddrDDduDotdr;
        ddrDDduduDot=ddrDDduDotdu;
        ddrDDdvduDot=ddrDDduDotdv;
        ddrDDduDotduDot=(2*r*diffV2)/w2;
        ddrDDdvDotduDot=(2*r*u*v)/w2;

        ddrDDdrdvDot=ddrDDdvDotdr;
        ddrDDdudvDot=ddrDDdvDotdu;
        ddrDDdvdvDot=ddrDDdvDotdv;
        ddrDDduDotdvDot=ddrDDdvDotduDot;
        ddrDDdvDotdvDot=(2*r*diffU2)/w2;

        %%%
        dduDDdrdr=-((4*rDot*uDot)/r^3);
        dduDDdudr=0;
        dduDDdvdr=0;
        dduDDdrDotdr=(2*uDot)/r2;
        dduDDduDotdr=(2*rDot)/r2;
        dduDDdvDotdr=0;

        dduDDdrdu=dduDDdudr;
        dduDDdudu=-(2*u*uDot2*(3+u2-3*v2)*diffV2-4*uDot*v*diffV2*(-1-3*u2+v2)*vDot+2*u*v2*(3+u2-3*v2)*vDot2)/w6;
        dduDDdvdu=-(2*u2*uDot2*v*(3+u2-3*v2)+4*u*uDot*(1-v4+u2*(-1+3*v2))*vDot+2*v*(1-u4+(-1+3*u2)*v2)*vDot2)/w6;
        dduDDdrDotdu=0;
        dduDDduDotdu=-(2*diffV2*(uDot*(1+u2-v2)+2*u*v*vDot))/w4;
        dduDDdvDotdu=(-4*u*uDot*v*diffV2-2*(diffU4+(-1+3*u2)*v2)*vDot)/w4;

        dduDDdrdv=dduDDdvdr;
        dduDDdudv=dduDDdvdu;
        dduDDdvdv=-(2*u*(u2*uDot2*(1-u2+3*v2)+2*u*uDot*v*(3-3*u2+v2)*vDot-diffU2*(-1+u2-3*v2)*vDot2))/w6;
        dduDDdrDotdv=0;
        dduDDduDotdv=(2*u2*(-2*u*uDot*v+u2*vDot-(1+v2)*vDot))/w4;
        dduDDdvDotdv=(2*u*(u*uDot*(-1+u2-v2)-2*diffU2*v*vDot))/w4;

        dduDDdrdRDot=dduDDdrDotdr;
        dduDDdudRDot=dduDDdrDotdu;
        dduDDdvdRDot=dduDDdrDotdv;
        dduDDdrDotdRDot=0;
        dduDDduDotdRDot=-(2/r);
        dduDDdvDotdRDot=0;

        dduDDdrduDot=dduDDduDotdr;
        dduDDduduDot=dduDDduDotdu;
        dduDDdvduDot=dduDDduDotdv;
        dduDDdrDotduDot=dduDDduDotdRDot;
        dduDDduDotduDot=-((2*u*diffV2)/w2);
        dduDDdvDotduDot=-(2*u2*v)/w2;

        dduDDdrdvDot=dduDDdvDotdr;
        dduDDdudvDot=dduDDdvDotdu;
        dduDDdvdvDot=dduDDdvDotdv;
        dduDDdrDotdvDot=dduDDdvDotdRDot;
        dduDDduDotdvDot=dduDDdvDotduDot;
        dduDDdvDotdvDot=-(2*u*diffU2)/w2;

        %%%
        ddvDDdrdr=-((4*rDot*vDot)/r^3);
        ddvDDdudr=0;
        ddvDDdvdr=0;
        ddvDDdrDotdr=(2*vDot)/r2;
        ddvDDduDotdr=0;
        ddvDDdvDotdr=(2*rDot)/r2;

        ddvDDdrdu=ddvDDdudr;
        ddvDDdudu=-(2*v*(uDot2*(-1+v)*(1+v)*(-1-3*u2+v2)+2*u*uDot*v*(3+u2-3*v2)*vDot+v2*(1+3*u2-v2)*vDot2))/w6;
        ddvDDdvdu=-(2*u*uDot2*(1-v4+u2*(-1+3*v2))+4*uDot*v*(1-u4+(-1+3*u2)*v2)*vDot+2*u*v2*(3-3*u2+v2)*vDot2)/w6;
        ddvDDdrDotdu=0;
        ddvDDduDotdu=(2*v*(2*u*uDot*(-diffV2)+v*(-1-u2+v2)*vDot))/w4;
        ddvDDdvDotdu=-((2*v2*(uDot*(1+u2-v2)+2*u*v*vDot))/w4);

        ddvDDdrdv=ddvDDdvdr;
        ddvDDdudv=ddvDDdvdu;
        ddvDDdvdv=-(2*u2*uDot2*v*(3-3*u2+v2)-4*u*diffU2*uDot*(-1+u2-3*v2)*vDot+2*diffU2*v*(3-3*u2+v2)*vDot2)/w6;
        ddvDDdrDotdv=0;
        ddvDDduDotdv=(-2*uDot*(diffV4+u2*(-1+3*v2))-4*u*diffU2*v*vDot)/w4;
        ddvDDdvDotdv=(2*diffU2*(-2*u*uDot*v+u2*vDot-(1+v2)*vDot))/w4;

        ddvDDdrdRDot=ddvDDdrDotdr;
        ddvDDdudRDot=ddvDDdrDotdu;
        ddvDDdvdRDot=ddvDDdrDotdv;
        ddvDDdrDotdRDot=0;
        ddvDDduDotdRDot=0;
        ddvDDdvDotdRDot=-(2/r);

        ddvDDdrduDot=ddvDDduDotdr;
        ddvDDduduDot=ddvDDduDotdu;
        ddvDDdvduDot=ddvDDduDotdv;
        ddvDDdrDotduDot=ddvDDduDotdRDot;
        ddvDDduDotduDot=-((2*v*diffV2)/w2);
        ddvDDdvDotduDot=-(2*u*v2)/w2;

        ddvDDdrdvDot=ddvDDdvDotdr;
        ddvDDdudvDot=ddvDDdvDotdu;
        ddvDDdvdvDot=ddvDDdvDotdv;
        ddvDDdrDotdvDot=ddvDDdvDotdRDot;
        ddvDDduDotdvDot=ddvDDdvDotduDot;
        ddvDDdvDotdvDot=((2*(-1+u^2)*v)/w2);

        aHess(:,:,1)=[0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrdr,       ddrDDdudr,      ddrDDdvdr,      0,              ddrDDduDotdr,   ddrDDdvDotdr;
                      dduDDdrdr,       dduDDdudr,      dduDDdvdr,      dduDDdrDotdr,   dduDDduDotdr,   dduDDdvDotdr;
                      ddvDDdrdr,       ddvDDdudr,      ddvDDdvdr,      ddvDDdrDotdr,   ddvDDduDotdr,   ddvDDdvDotdr];
        aHess(:,:,2)=[0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrdu,       ddrDDdudu,      ddrDDdvdu,      0,              ddrDDduDotdu,   ddrDDdvDotdu;
                      dduDDdrdu,       dduDDdudu,      dduDDdvdu,      dduDDdrDotdu,   dduDDduDotdu,   dduDDdvDotdu;
                      ddvDDdrdu,        ddvDDdudu,     ddvDDdvdu,      ddvDDdrDotdu,   ddvDDduDotdu,   ddvDDdvDotdu];
        aHess(:,:,3)=[0,               0,              0,              0               0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrdv,       ddrDDdudv,      ddrDDdvdv,      0,              ddrDDduDotdv,   ddrDDdvDotdv;
                      dduDDdrdv,       dduDDdudv,      dduDDdvdv,      dduDDdrDotdv,   dduDDduDotdv,   dduDDdvDotdv;
                      ddvDDdrdv,       ddvDDdudv,      ddvDDdvdv,      ddvDDdrDotdv,   ddvDDduDotdv,   ddvDDdvDotdv];
        aHess(:,:,4)=[0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrdRDot,    ddrDDdudRDot,   ddrDDdvdRDot,   0,              ddrDDduDotdRDot,ddrDDdvDotdRDot;
                      dduDDdrdRDot,    dduDDdudRDot,   dduDDdvdRDot,   dduDDdrDotdRDot,dduDDduDotdRDot,dduDDdvDotdRDot;
                      ddvDDdrdRDot,    ddvDDdudRDot,   ddvDDdvdRDot,   ddvDDdrDotdRDot,ddvDDduDotdRDot,ddvDDdvDotdRDot];
        aHess(:,:,5)=[0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrduDot,    ddrDDduduDot,   ddrDDdvduDot,   0,              ddrDDduDotduDot,ddrDDdvDotduDot;
                      dduDDdrduDot,    dduDDduduDot,   dduDDdvduDot,   dduDDdrDotduDot,dduDDduDotduDot,dduDDdvDotduDot;
                      ddvDDdrduDot,    ddvDDduduDot,   ddvDDdvduDot,   ddvDDdrDotduDot,ddvDDduDotduDot,ddvDDdvDotduDot];
        aHess(:,:,6)=[0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrdvDot,    ddrDDdudvDot,   ddrDDdvdvDot,   0,              ddrDDduDotdvDot,ddrDDdvDotdvDot;
                      dduDDdrdvDot,    dduDDdudvDot,   dduDDdvdvDot,   dduDDdrDotdvDot,dduDDduDotdvDot,dduDDdvDotdvDot;
                      ddvDDdrdvDot,    ddvDDdudvDot,   ddvDDdvdvDot,   ddvDDdrDotdvDot,ddvDDduDotdvDot,ddvDDdvDotdvDot];

        if(nargout>3)
            papt=zeros(6,1);
        end
    end
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 degree = r8poly_degree ( na, a )

%*****************************************************************************80
%
%% R8POLY_DEGREE returns the degree of a polynomial.
%
%  Discussion:
%
%    The degree of a polynomial is the index of the highest power
%    of X with a nonzero coefficient.
%
%    The degree of a constant polynomial is 0.  The degree of the
%    zero polynomial is debatable, but this routine returns the
%    degree as 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NA, the dimension of A.
%
%    Input, real A(NA+1), the coefficients of the polynomials.
%
%    Output, integer DEGREE, the degree of A.
%
  degree = na;

  while ( 0 < degree )

    if ( a(degree+1) ~= 0.0 )
      return
    end

    degree = degree - 1;

  end

  return
end


function [H] = globalrescale(f);

% GLOBALRESCALE creates the homogenous spatial transformation matrix
% for a 7 parameter rigid-body transformation with global rescaling
%
% Use as
%   [H] = globalrescale(f)
%
% The transformation vector f should contain the
%   x-shift
%   y-shift
%   z-shift
% followed by the
%   pitch (rotation around x-axis)
%   roll  (rotation around y-axis)
%   yaw   (rotation around z-axis)
% followed by the
%   global rescaling factor

% Copyright (C) 2000-2005, Robert Oostenveld
%
% 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

% $Log: globalrescale.m,v $
% Revision 1.1  2009/01/30 04:02:06  arno
% *** empty log message ***
%
% Revision 1.3  2005/08/15 08:15:32  roboos
% reimplemented the rotate function, which contained an error (the error is in the AIR technical reference)
% changed all functions to be dependent on the rotate, translate and scale function
% all functions now behave consistenly, which also means that they are not compleetly backward compatible w.r.t. the order of the rotations
%
% Revision 1.2  2004/05/19 09:57:07  roberto
% added GPL copyright statement, added CVS log item
%

% compute the homogenous transformation matrix for the translation
T = translate(f([1 2 3]));

% compute the homogenous transformation matrix for the rotation
R = rotate_new(f([4 5 6]));

% compute the homogenous transformation matrix for the global scaling
S = scale(f([7 7 7]));

% compute the homogenous transformation matrix for the combination
H = T*R*S;


function h = plot_gaussian_ellipsoid(m, C, sdwidth, npts, axh,alphaChannel,color)
% PLOT_GAUSSIAN_ELLIPSOIDS plots 2-d and 3-d Gaussian distributions
%
% H = PLOT_GAUSSIAN_ELLIPSOIDS(M, C) plots the distribution specified by
%  mean M and covariance C. The distribution is plotted as an ellipse (in
%  2-d) or an ellipsoid (in 3-d).  By default, the distributions are
%  plotted in the current axes. H is the graphics handle to the plotted
%  ellipse or ellipsoid.
%
% PLOT_GAUSSIAN_ELLIPSOIDS(M, C, SD) uses SD as the standard deviation
%  along the major and minor axes (larger SD => larger ellipse). By
%  default, SD = 1. Note:
%  * For 2-d distributions, SD=1.0 and SD=2.0 cover ~ 39% and 86%
%     of the total probability mass, respectively.
%  * For 3-d distributions, SD=1.0 and SD=2.0 cover ~ 19% and 73%
%     of the total probability mass, respectively.
%
% PLOT_GAUSSIAN_ELLIPSOIDS(M, C, SD, NPTS) plots the ellipse or
%  ellipsoid with a resolution of NPTS (ellipsoids are generated
%  on an NPTS x NPTS mesh; see SPHERE for more details). By
%  default, NPTS = 50 for ellipses, and 20 for ellipsoids.
%
% PLOT_GAUSSIAN_ELLIPSOIDS(M, C, SD, NPTS, AX) adds the plot to the
%  axes specified by the axis handle AX.
%
% Examples:
% -------------------------------------------
%  % Plot three 2-d Gaussians
%  figure;
%  h1 = plot_gaussian_ellipsoid([1 1], [1 0.5; 0.5 1]);
%  h2 = plot_gaussian_ellipsoid([2 1.5], [1 -0.7; -0.7 1]);
%  h3 = plot_gaussian_ellipsoid([0 0], [1 0; 0 1]);
%  set(h2,'color','r');
%  set(h3,'color','g');
%
%  % "Contour map" of a 2-d Gaussian
%  figure;
%  for sd = [0.3:0.4:4],
%    h = plot_gaussian_ellipsoid([0 0], [1 0.8; 0.8 1], sd);
%  end
%
%  % Plot three 3-d Gaussians
%  figure;
%  h1 = plot_gaussian_ellipsoid([1 1  0], [1 0.5 0.2; 0.5 1 0.4; 0.2 0.4 1]);
%  h2 = plot_gaussian_ellipsoid([1.5 1 .5], [1 -0.7 0.6; -0.7 1 0; 0.6 0 1]);
%  h3 = plot_gaussian_ellipsoid([1 2 2], [0.5 0 0; 0 0.5 0; 0 0 0.5]);
%  set(h2,'facealpha',0.6);
%  view(129,36); set(gca,'proj','perspective'); grid on;
%  grid on; axis equal; axis tight;
% -------------------------------------------
%
%  Gautam Vallabha, Sep-23-2007, Gautam.Vallabha@mathworks.com

%  Revision 1.0, Sep-23-2007
%    - File created
%  Revision 1.1, 26-Sep-2007
%    - NARGOUT==0 check added.
%    - Help added on NPTS for ellipsoids

if ~exist('sdwidth', 'var'), sdwidth = 1; end
if ~exist('npts', 'var'), npts = []; end
if ~exist('axh', 'var'), axh = gca; end
if ~exist('alphaChannel','var'),alphaChannel=1;end
if ~exist('color','var'),color=[0 0 1];end

if numel(m) ~= length(m),
    error('M must be a vector');
end
if ~( all(numel(m) == size(C)) )
    error('Dimensionality of M and C must match');
end
if ~(isscalar(axh) && ishandle(axh) && strcmp(get(axh,'type'), 'axes'))
    error('Invalid axes handle');
end

set(axh, 'nextplot', 'add');

switch numel(m)
%   case 2, if alphaChannel,  h=show2d_noalpha(m(:),C,sdwidth,npts,axh,color); else h=show2d(m(:),C,sdwidth,npts,axh,alphaChannel,color); end
    case 2, h=show2d(m(:),C,sdwidth,npts,axh,alphaChannel,color);
    case 3, h=show3d(m(:),C,sdwidth,npts,axh);
   otherwise
      error('Unsupported dimensionality');
end

if nargout==0,
    clear h;
end

%-----------------------------
function h = show2d(means, C, sdwidth, npts, axh,alphaChannel,color)
if isempty(npts), npts=50; end
% plot the gaussian fits
tt=linspace(0,2*pi,npts)';
x = cos(tt); y=sin(tt);
ap = [x(:) y(:)]';
[v,d]=eig(C);
d = sdwidth * sqrt(d); % convert variance to sdwidth*sd
bp = (v*d*ap) + repmat(means, 1, size(ap,2));
%if alphaChannel == 1
%h = plot(bp(1,:), bp(2,:), '-', 'parent', axh);
%set(h,'color',color);
%else
h = patchline(bp(1,:),bp(2,:),'linewidth',2,'linestyle','-','edgealpha',alphaChannel,'edgecolor',color,'LineSmoothing','on');
%end

function h = show2d_noalpha(means, C, sdwidth, npts, axh,color)
if isempty(npts), npts=50; end
% plot the gaussian fits
tt=linspace(0,2*pi,npts)';
x = cos(tt); y=sin(tt);
ap = [x(:) y(:)]';
[v,d]=eig(C);
d = sdwidth * sqrt(d); % convert variance to sdwidth*sd
bp = (v*d*ap) + repmat(means, 1, size(ap,2));
h = plot(bp(1,:), bp(2,:), '-', 'parent', axh);
set(h,'color',color);

%-----------------------------
function h = show3d(means, C, sdwidth, npts, axh)
if isempty(npts), npts=20; end
[x,y,z] = sphere(npts);
ap = [x(:) y(:) z(:)]';
[v,d]=eig(C);
if any(d(:) < 0)
   fprintf('warning: negative eigenvalues\n');
   d = max(d,0);
end
d = sdwidth * sqrt(d); % convert variance to sdwidth*sd
bp = (v*d*ap) + repmat(means, 1, size(ap,2));
xp = reshape(bp(1,:), size(x));
yp = reshape(bp(2,:), size(y));
zp = reshape(bp(3,:), size(z));
h = surf(axh, xp,yp,zp);


%RPY2R Roll-pitch-yaw angles to rotation matrix
%
% R = RPY2R(ROLL, PITCH, YAW, OPTIONS) is an SO(3) orthonornal rotation
% matrix (3x3) equivalent to the specified roll, pitch, yaw angles angles.
% These correspond to rotations about the X, Y, Z axes respectively. If
% ROLL, PITCH, YAW are column vectors (Nx1) then they are assumed to
% represent a trajectory and R is a three-dimensional matrix (3x3xN), where
% the last index corresponds to rows of ROLL, PITCH, YAW.
%
% R = RPY2R(RPY, OPTIONS) as above but the roll, pitch, yaw angles angles
% angles are taken from consecutive columns of the passed matrix RPY =
% [ROLL, PITCH, YAW].  If RPY is a matrix (Nx3) then they are assumed to
% represent a trajectory and R is a three-dimensional matrix (3x3xN), where
% the last index corresponds to rows of RPY which are assumed to be [ROLL,
% PITCH, YAW].
%
% Options::
%  'deg'   Compute angles in degrees (radians default)
%  'zyx'   Return solution for sequential rotations about Z, Y, X axes (Paul book)
%
% Note::
% - In previous releases (<8) the angles corresponded to rotations about ZYX. Many
%   texts (Paul, Spong) use the rotation order ZYX. This old behaviour can be enabled
%   by passing the option 'zyx'
%
% See also TR2RPY, EUL2TR.

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

function R = rpy2r(roll, varargin)
    opt.zyx = false;
    opt.deg = false;
    [opt,args] = tb_optparse(opt, varargin);

    % unpack the arguments
    if numcols(roll) == 3
		pitch = roll(:,2);
		yaw = roll(:,3);
		roll = roll(:,1);
	elseif nargin >= 3
        pitch = args{1};
        yaw = args{2};
    else
        error('RTB:rpy2r:badarg', 'bad arguments')
    end

    % optionally convert from degrees
    if opt.deg
        d2r = pi/180.0;
        roll = roll * d2r;
        pitch = pitch * d2r;
        yaw = yaw * d2r;
    end

    if ~opt.zyx
        % XYZ order
        if numrows(roll) == 1
            R = rotx(roll) * roty(pitch) * rotz(yaw);
        else
            R = zeros(3,3,numrows(roll));
            for i=1:numrows(roll)
                R(:,:,i) = rotx(roll(i)) * roty(pitch(i)) * rotz(yaw(i));
            end
        end
    else
        % old ZYX order (as per Paul book)
        if numrows(roll) == 1
            R = rotz(roll) * roty(pitch) * rotx(yaw);
        else
            R = zeros(3,3,numrows(roll));
            for i=1:numrows(roll)
                R(:,:,i) = rotz(roll(i)) * roty(pitch(i)) * rotx(yaw(i));
            end
        end
    end


function G = gaborPyramid(s, theta, sz);
% Create a gabor wavelet pyramid.
%
% G = gaborPyramid(<s=0:2>, <theta=22.5:22.5:180>, <sz=[256 256]>);
%
% BACKGROUND: this function creates a wavelet pyramid analogous to that
% used by Kay et al (Nature 2000) to represent images for application to
% fMRI activity patterns. This pyramid contains a series of Gabor wavelets,
% each similar to the one described by Tai Sing Lee, "Image Representation
% Using 2D Gabor Wavelets".
% (IEEE Transactions 1996: online at www.cnbc.cmu.edu/~tai/papers/pami.pdf)
%
% "Gabor" wavelets (or "Weyl-Heisenberg groups" in physics) are a combined
% harmonic and Gaussian function which the physicist Dennis Gabor showed
% convey an optimal combination of spatial (or temporal) position localization
% and frequency-domain localization. These functions serve as models of
% receptive fields for early visual neurons, esp. simple cells. See the Lee
% paper for more background.
%
% "Pyramid" refers to the fact that, for different spatial frequencies,
% there will be a different number of channels, spanning the different
% positions that can be localized. This code follows the Kay et al
% convention of having spatial frequencies be powers of 2 in cycles per
% image: for 2 cycles per image, there will be 2^2=4 possible positions;
% for 4 cycles per image, there will be 4^2 possible positions.
%
%
% INPUTS:
% s: vector of spatial frequencies to use. s should be an integer, and specify
% the log base 2 of the spatial frequency, in units of cycles / image. That is,
% 2^s is the number of cycles in the image for a given channel.
% E.g.: s=0 indicates a single cycle per image; s=1 indicates two cycles
% per image. A DC offset is automatically added to the list of channels as well,
% which has undefined spatial frequency (2^s = 0).
%
% theta: vector of orientations to use, in degrees counterclockwise from
% the x0 axis. That is, 0=horizontal, 45 = increasing in x0/y0 direction, 90 =
% vertical, 135 = increasing in x0 while decreasing in y0 direction. Note
% that theta is cyclic with modulo 180 (theta=200 is the same as theta=20).
%
% sz: size [x y] of the resulting wavelets.
%
% OUTPUTS:
% Return results in a structure G, with the following fields:
%	odd: 3-D matrix of size [imgSize(1) imgSize(2) nChannels].
%		odd-symmetric (sine) component channels.
%		The # of channels depends on omega and theta (see above), and
%		represents the even channels only in the complex function.
%	even: same size as odd, containing even-symmetric (cosine) component
%	channels.
%	cyclesPerImage: vector of wavelengths corresponding to each channel.
%	orientation: vector of orientations corresponding to each channel.
%	x0: vector of x0 position centers for each channel
%	y0: vector of y0 position centers for each channel.
%
% REFERENCES: We implement a version of the wavelet decomposition used in
% Kay et al., Nature 2008. See also www.cnbc.cmu.edu/~tai/papers/pami.pdf.
%
% ras, 03/2008.
if notDefined('s'),			s = 0:2;					end
if notDefined('theta'),		theta = 22.5:22.5:180;		end
if notDefined('sz'),		sz = [128 128];				end

%%%%% params / setup
%% constants
kappa = 2.5; % pi;  % constrains the spat. freq. bandwith to be 1 octave

%% parse inputs
if length(sz)==1  % square image, only 1 size specified
	sz = [sz sz];
end

%% init empty struct
G.odd = [];
G.even = [];
G.layer = [];
G.cyclesPerImage = [];
G.orientation = [];
G.x0 = [];
G.y0 = [];

%%%%% (1) determine the number of channels
% first, convert the spatial frequency param s into units of cycles per
% image (cpi):
cpi = 2 .^ s;

% we add one channel for the DC component:
cpi = [0 cpi];

% for each # of cycles/image, we have a square grid of positions for that
% frequency (see Kay et al, Sup Fig 2).
tmp = sum( cpi .^ 2 ); % # of joint spatial-frequency/position combinations

% for each layer except the first one, we have one channel for each
% orientation. So, figure out the total # of channels:
nChannels = tmp * length(theta) + 1;
G.nChannels = nChannels;

%%%%% (2) determine parameters for each channel
% now, assign to each channel a unique combination of spatial frequency
% (cyclesPerImage), orientation, and position (x0 and y0). We'll call the
% set of channels corresponding to this spat. freq a "layer" in the pyramid:
for layer = 1:length(cpi)
	% the 0 cycles/image layer is a special case: only 1 DC channel
	if cpi(layer)==0
		G.layer = 1;
		G.cyclesPerImage = 0;
		G.orientation = 0; % could be NaN; this will help w/ plotting
		G.x0 = 0;
		G.y0 = 0;

	else
		% # positions along x/y axis for this layer
		nGrid = cpi(layer);

		% # channels in this layer
		n = length(theta) * nGrid ^ 2;

		% append cpi, orientation values
		G.layer = [G.layer repmat(layer, [1 n])];
		G.cyclesPerImage = [ G.cyclesPerImage, repmat(cpi(layer), [1 n]) ];
		G.orientation = [ G.orientation, repmat(theta, [1 nGrid^2]) ];

		% get a grid of (x0, y0) centers for each position for this layer
		% these positions are in pixels from the uppper left-hand corner of
		% the image:
		yRange = linspace(0, sz(1), nGrid+2);  yRange = yRange(2:end-1);
		xRange = linspace(0, sz(2), nGrid+2);  xRange = xRange(2:end-1);
		[x0 y0] = meshgrid( xRange, yRange );

		% we tile the position space separately for each orientation
		x0 = repmat(x0(:)', [length(theta) 1]);
		y0 = repmat(y0(:)', [length(theta) 1]);
		G.x0 = [G.x0, x0(:)'];
		G.y0 = [G.y0, y0(:)'];
	end
end

%%%%% (3) create the channels
% initialize the empty channels
G.odd	= zeros(sz(1), sz(2), nChannels);
G.even	= zeros(sz(1), sz(2), nChannels);

% main loop
for n = 1:nChannels
	% get a map of (x,y) pixel positions relative to the center of the
	% Gabor function for this channel (x0, y0) across the image
	[x y] = meshgrid( [1:sz(2)] - G.x0(n), [1:sz(1)] - G.y0(n) );

	% express the orientation in radians
	theta = G.orientation(n) * pi/180;

	% convert spatial frequency from units of cycles per image to units of
	% radians per unit distance: we take as the image size the larger of
	% the two edge lengths for the image:
	omega = 2*pi * G.cyclesPerImage(n)*2 / max(sz);

	% compute the wavelet
	phi = gaborWavelet(x, y, theta, omega, kappa);

	% grab the odd and even components
	G.even(:,:,n)	= real(phi);
	G.odd(:,:,n)	= imag(phi);
end

% to save memory, let's make everything single-precision
for f = fieldnames(G)'
	G.(f{1}) = single( G.(f{1}) );
end

return


%% DEMO 9:  Algorithms 04. Total Variation minimization algorithms
%
%
%  This demo presents the Total variation algorithms in TIGRE. Total
%  variation algorithms try to minimize the variation (gradient) of the
%  image, assuming its piecewise smooth, as most things in nature are (i.e.
%  human body).
%
% This set of algorithms is specially good performing when the noise is
% very big or the number of projections is small, however, they require more
% computational time and memory than the other algorithms to run.
%
%
% This Demo is not fully complete, as several algorithms are not shown
% here, yet are available in TIGRE, namely:
%
% - AwASD_POCS:  Edge preserving ASD_POCS (Adaptative weigthed).
% - OS_AwASD_POCS: OS-version of the previous algorithm
% - PCSD: A version of ASD_POCS that heuristically select some of the
%         parameters, particularly epsilon (maxL2norm)
% - AwPCSD: Edge preserving version of the previous algorithm
% - OS_AwPCSD: block-wise version of the previous
%
% You can use these algorithms in a very similar manner as below examples.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------
%% Initialize
clear;
close all;

%% Define Geometry
geo=defaultGeometry('nVoxel',[128;128;128]);

%% Load data and generate projections
% see previous demo for explanation
angles=linspace(0,2*pi-2*pi/30,30);
head=headPhantom(geo.nVoxel);
projections=Ax(head,geo,angles,'interpolated');
noise_projections=addCTnoise(projections);

%% Lets create a OS-SART test for comparison
[imgOSSART,errL2OSSART]=OS_SART(noise_projections,geo,angles,60);

%% Total Variation algorithms
%
%  ASD-POCS: Adaptative Steeppest Descent-Projection On Convex Subsets
% Often called POCS-TV
%==========================================================================
%==========================================================================
%  ASD-POCS minimizes At-B and the TV norm separately in each iteration,
%  i.e. reconstructs the image first and then reduces the TV norm, every
%  iteration. As the other algorithms the mandatory inputs are projections,
%  geometry, angles and maximum iterations.
%
% ASD-POCS has a veriety of optional arguments, and some of them are crucial
% to determine the behaviour of the algorithm. The advantage of ASD-POCS is
% the power to create good images from bad data, but it needs a lot of
% tunning.
%
%  Optional parameters that are very relevant:
% ----------------------------------------------
%    'maxL2err'    Maximum L2 error to accept an image as valid. This
%                  parameter is crucial for the algorithm, determines at
%                  what point an image should not be updated further.
%                  Default is 20% of the FDK L2 norm.
%
% its called epsilon in the paper
epsilon=im3Dnorm(Ax(FDK(noise_projections,geo,angles),geo,angles)-noise_projections,'L2')*0.15;
%   'alpha':       Defines the TV hyperparameter. default is 0.002.
%                  However the paper mentions 0.2 as good choice
alpha=0.002;

%   'TViter':      Defines the amount of TV iterations performed per SART
%                  iteration. Default is 20

ng=25;

% Other optional parameters
% ----------------------------------------------
%   'lambda':      Sets the value of the hyperparameter for the SART iterations.
%                  Default is 1
%
%   'lambdared':   Reduction of lambda Every iteration
%                  lambda=lambdared*lambda. Default is 0.99
%
lambda=1;
lambdared=0.9999;

%   'alpha_red':   Defines the reduction rate of the TV hyperparameter
alpha_red=0.95;

%   'Ratio':       The maximum allowed image/TV update ration. If the TV
%                  update changes the image more than this, the parameter
%                  will be reduced.default is 0.95
ratio=0.94;

%   'Verbose'      1 or 0. Default is 1. Gives information about the
%                  progress of the algorithm.

verb=true;

imgASDPOCS=ASD_POCS(noise_projections,geo,angles,50,...
                    'TViter',ng,'maxL2err',epsilon,'alpha',alpha,... % these are very important
                     'lambda',lambda,'lambda_red',lambdared,'Ratio',ratio,'Verbose',verb); % less important.

%  OS_ASD_POCS: Odered Subset-TV algorithm
%==========================================================================
%==========================================================================
%
% The logical next step to imporce ASD-POCS is substituting SART with a
% faster algorithm, such as OS-SART
%
% The parameters are the same as in ASD-POCS, but also have 'BlockSize' and
% @OrderStrategy', taken from OS-SART

imgOSASDPOCS=OS_ASD_POCS(noise_projections,geo,angles,50,...
                    'TViter',ng,'maxL2err',epsilon,'alpha',alpha,... % these are very important
                     'lambda',lambda,'lambda_red',lambdared,'Ratio',ratio,'Verbose',verb,...% less important.
                      'BlockSize',size(angles,2)/10,'OrderStrategy','angularDistance'); %OSC options

%  B-ASD-POCS-beta
%==========================================================================
%==========================================================================
% Is another version of ASD-POCS that has uses Bregman iteration, i.e.
% updates the data vector every some ASD-POCS iteration, bringing closer
% the data to the actual image. According to the original article, this
% accelerates convergence rates, giving a better image in the same amount
% of iteratiosn.
%
% It has all the inputs from ASD-POCS, and has 3 extra:
%   'beta'         hyperparameter controling the Bragman update. default=1
%
%   'beta_red'     reduction of the beta hyperparameter. default =0.75
%
%   'bregman_iter' amount of global bregman iterations. This will define
%                  how often the bregman iteration is executed. It has to
%                  be smaller than the number of iterations.

%Note that the number of iteration for TV has changed

imgBASDPOCSbeta=B_ASD_POCS_beta(noise_projections,geo,angles,50,...
                    'TViter',40,'maxL2err',epsilon,'alpha',alpha,... % these are very important
                     'lambda',lambda,'lambda_red',lambdared,'Ratio',ratio,'Verbose',verb,... % less important.
                      'beta',0.5,'beta_red',0.7,'bregman_iter',10); % bregman options

%   SART-TV
%==========================================================================
%==========================================================================
%
%   This implementation differs more from the others, as it minimizes the
%   ROF model, i.e. when minimizing the total variation of the image, it
%   also takes into account the information of the image. If only the total
%   variation minimization step was run in the rest of the algorithms, the
%   result would be a flat image (as that is the minimum total variation
%   image), altertatively, the ROF model enforces the image not to change too
%   much.
%
%   This algirths performs better with more projections, but noisy data, by
%   enforncing the TV very little
%
%   The optional parameters are for the total variatiot part of the image:
%
%
%
%   'TViter'       amoutn of iteration in theTV step. Default 50
%
%   'TVlambda'     hyperparameter in TV iteration. IT gives the ratio of
%                  importance of the image vs the minimum total variation.
%                  default is 15. Lower means more TV denoising.
%

imgSARTTV=SART_TV(noise_projections,geo,angles,50,'TViter',100,'TVlambda',50);

% IRN_TV_CGLS
%==========================================================================
%==========================================================================
% CGLS with TV regularization in an inner/outer iteration scheme
%
% 'lambda' hyperparameter in TV norm. It gives the ratio of
%          importance of the image vs the minimum total variation.
%          default is 15. Lower means less TV denoising.
%
% 'niter_outer' Number of outer iterations. Each outer iteration will
%               perform niter number of inner iterations, in the example
%               below, 20.Albeit this seems that it does many more
%               iterations than the other algorithms, this is an inherently
%               faster algorithm, both in convergence and time.

imgIRN_TV_CGLS=IRN_TV_CGLS(noise_projections,geo,angles,20,'lambda',5,'niter_outer',2);
% hybrid_fLSQR_TV
%==========================================================================
%==========================================================================
% flexyble hybrod LSQR. Has TV regularization with reorthogonalization.
%
% 'lambda' hyperparameter in TV norm. It gives the ratio of
%          importance of the image vs the minimum total variation.
%          default is 15. Lower means less TV denoising.
%
imgflsqr=hybrid_fLSQR_TV(projections,geo,angles,20,'lambda',5);

 %% Lets visualize the results
% Notice the smoother images due to TV regularization.
%
%     head              OS-SART           ASD-POCS
%
%     OS-ASD-POCS       B-ASD-POCS-beta   SART-TV
%
%     IRN-TV-CGLS        hybrid-fLSQR       heads

plotImg([imgIRN_TV_CGLS imgflsqr  head;
         imgOSASDPOCS imgBASDPOCSbeta imgSARTTV;
         head, imgOSSART, imgASDPOCS ] ,'Dim','Z','Step',2,'clims',[0 1])
 % error
plotImg(abs([head-imgIRN_TV_CGLS head-imgflsqr  head-head;
         head-imgOSASDPOCS head-imgBASDPOCSbeta head-imgSARTTV;
         head-head, head-imgOSSART, head-imgASDPOCS ]) ,'Dim','Z','Slice',64,'clims',[0 0.1])

%%
% Obligatory XKCD reference: https://xkcd.com/833/