function cdf = frechet_cdf ( x, alpha )

%*****************************************************************************80
%
%% FRECHET_CDF evaluates the Frechet CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the parameter.
%    It is required that 0.0 < ALPHA.
%
%    Input, real X, the argument of the CDF.
%
%    Output, real CDF, the value of the CDF.
%
  if ( alpha <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FRECHET_CDF - Fatal error!\n' );
    fprintf ( 1, '  ALPHA <= 0.0.\n' );
    error ( 'FRECHET_CDF - Fatal error!' );
  end

  if ( x <= 0.0 )
    cdf = 0.0;
  else
    cdf = exp ( - 1.0 / x^alpha );
  end

  return
end


function [P,exitCode]=RiccatiPostNoClutter(H,F,R,Q,PD,RelTol,AbsTol,maxIter)
%%RICCATIPOSTNOCLUTTER Solve the posterior Riccati equation that has been
%                      modified for a probability of detection less
%                      than or equal to 1. This provides the average
%                      asymptotic covariance of a linear Kalman filter with
%                      linear measurements just after a (possible with
%                      probability PD) measurement update.
%
%INPUTS: H The zDim X xDim measurement matrix such that H*x+w is the
%          measurement, where x is the state and w is zero-mean Gaussian
%          noise with covariance matrix R.
%        F The xDim X xDim state transition matrix The state at discrete-
%          time k+1 is modeled as F times the state at time k plus zero-
%          mean Gaussian process noise with covariance matrix Q.
%        R The zDim X zDim measurement covariance matrix.
%        Q The xDim X xDim process noise covariance matrix. This can not be
%          a singular matrix.
%       PD The optional detection probability of the target at each scan.
%          If omitted, PD is assumed to be one.
%   RelTol The maximum relative error tolerance allowed, a positive scalar.
%          This tolerance applies to all elements in the matrix and is only
%          needed if PD<1. If omitted or an empty matrix is passed, the
%          default value of 1e-13 is used.
%   AbsTol The absolute error tolerance allowed, a positive scalar. This
%          tolerance applies to all elements in the matrix and is only
%          needed if PD<1. If omitted or an empty matrix is passed, the
%          default value of 1e-10 is used.
%  maxIter An optional integer specifying the maximum number of iterations.
%          If omitted or an empty matrix is passed, the default is 5000.
%
%OUTPUTS: P The average asymptotic error covariance matrix of the linear
%           Kalman filter with a probability of detection of PD after a
%           (possible) measurement update.
%  exitCode This is zero if convergence is achieved and 1 if it is not
%           achieved.
%
%The notion of a Riccati equation with a detection probability less than
%one is discussed in [1]. However, they do not explicitly write out the
%posterior form. The posterior recursion goes from P_{k|k} to P_{k+1|k+1}.
%This stands in contrast to the prior Riccati equation, which expresses the
%transition from P_{k|k-1} to P_{k+1|k}.
%
%The posterior Riccati equation is
%P=F*P*F'+Q-PD*(F*P*F'*H'+Q*H')*inv(H*F*P*F'*H'+H*Q*H'+R)*(F*P*F'*H'+Q*H')'
%and is derived by substituting the prediction step into the measurement
%update step of the linear Kalman filter with additive process noise and
%additive measurement noise.
%
%EXAMPLE:
% T=1;
% F=FPolyKal(T,6,1);
% q0=1;
% Q=QPolyKal(T,6,1,q0);
% R=diag([10;10;10]);
% H=[1,0,0,0,0,0;
%    0,1,0,0,0,0;
%    0,0,1,0,0,0];
% PD=0.5;
% P=RiccatiPostNoClutter(H,F,R,Q,PD)
%
%REFERENCES:
%[1] Y. Boers and H. Driessen, "Modified Riccati equation and its
%    application to target tracking," IEE Proceedings Radar, Sonar and
%    Navigation, vol. 153, no. 1, pp. 7-12, Feb. 2006.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(PD))
   PD=1;
end

if(nargin<6||isempty(AbsTol))
    AbsTol=1e-13;
end

if(nargin<7||isempty(RelTol))
    RelTol=1e-10;
end

if(nargin<8||isempty(maxIter))
   maxIter=5000;
end

%Get an initial estimate by solving the Riccati equation with PD=1.
AD=F';
BD=F'*H';
SD=Q*H';
RD=H*Q*H'+R;
QD=Q;
PPrev=RiccatiSolveD(AD,BD,QD,RD,SD);
exitCode=0;
if(PD~=1)
    %Now, iterate the Riccati equation with PD<1 until convergence of the
    %Frobenis norm or until a maximum number of iterations has occurred.
    curIter=0;
    while(curIter<maxIter)
        P=F*PPrev*F'+Q-PD*(F*PPrev*F'*H'+Q*H')*inv(H*F*PPrev*F'*H'+H*Q*H'+R)*(F*PPrev*F'*H'+Q*H')';
        %Ensure symmetry is preserved.
        P=(P+P')/2;

        diffMag=abs(P-PPrev);
        if(all((diffMag(:)<=RelTol*abs(P(:)))|(diffMag(:)<=AbsTol)))
            return;
        end

        PPrev=P;
        curIter=curIter+1;
    end
    exitCode=1;%Convergence not achieved.
else
    P=PPrev;
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 a = sylvester_kac_inverse ( n )

%*****************************************************************************80
%
%% SYLVESTER_KAC_INVERSE returns the inverse of the SYLVESTER_KAC matrix.
%
%  Example:
%
%    N = 6:
%
%      0     1/5    0  -2/15  0   8/15
%      1      0     0    0    0    0
%      0      0     0   1/3   0  -4/3
%    -4/3     0    1/3   0    0    0
%      0      0     0    0    0    1
%     8/15    0   -2/15  0   1/5   0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  if ( mod ( n, 2 ) == 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SYLVESTER_KAC_INVERSE - Fatal error!\n' );
    fprintf ( 1, '  The matrix is singular for odd N.\n' );
    error ( 'SYLVESTER_KAC_INVERSE - Fatal error!' );
  end

  a = zeros ( n, n );

  for i = 1 : n

    if ( mod ( i, 2 ) == 1 )

      for j = i : 2 : n-1

        if ( j == i )
          p1 = 1.0 / ( n - j );
          p2 = 1.0 /       j;
        else
          p1 = - p1 * (     j - 1 ) / ( n - j );
          p2 = - p2 * ( n - j + 1 ) /       j;
        end

        a(i,j+1) = p1;
        a(j+1,i) = p2;

      end

    end

  end

  return
end


function [rotmat,cornerpoints,volume,surface,edgelength] = minboundbox(x,y,z,metric,level)
% minboundbox: Compute the minimal bounding box of points in 3d
% usage: [rotmat,cornerpoints,volume,surface,edgelength] = minboundbox(x,y,z,metric,level)
%
% arguments: (input)
%  x,y,z - vectors of points, describing points in 3d as
%        (x,y,z) pairs. x, y and z must be the same lengths.
%
%  metric - (OPTIONAL) - single letter character flag which
%        denotes the use of minimal volume, surface or sum of edges as the
%        metric to be minimized. metric may be either 'v', 's' or 'e',
%        capitalization is ignored.
%
%        DEFAULT: 'v'    ('volume')
%
%  level - (OPTIONAL) - either 1, 2, 3 or 4. This denotes the level of
%       reliability of the resulting minimal bounding box.
%       '1' denotes the search for a bounding box, where one side of the
%           box coincides with a face of the convex hull. (fast)
%       '2' like '1', but also incorporates a search through each pair of edges
%           which form a plane which coincides with one side of the box (slow)
%       '3' like '1', but also incorporates a search through each edge which is
%           parallel to an edge of the box (reasonably fast)
%       '4' like '1', '2' and '3' together. (slowest) (Never needed that.)
%
%       It depends on the application, what should be chosen here.
%       See the example at the end for the effects of this parameter.
%
%        DEFAULT: '3'
%
% arguments: (output)
%  rotmat - (3x3) rotation matrix for mapping of the pointcloud into a
%                 box which is axis-parallel (use inv(rotmat) for inverse
%                 mapping).
%
%  cornerpoints - (8x3) the cornerpoints of the bounding box.
%
%  volume - (scalar) volume of the minimal box itself.
%
%  surface - (scalar) surface of the minimal box as found.
%
%  edgelength - (scalar) sum of the edgelengths of the minimal box as found.
%
% Thanks to John d'Errico for providing the solution to the 2d version of
% this problem via minboundrect from the FEX. I took a huge amount of his
% syntax, organisation and comments for this submission. (Hope he wont sue
% me, though.)
%
% Thanks to Roger Stafford and John d'Errico for helpful discussions and
% suggestions.
%
% The algorithm is still not proven to produce the smallest possible
% enclosing box. But i never found any counterexamples for the 'best'
% algorithm with level 4, whilst the second example below shows, there are
% differences between the levels. I would be happy to have a proof that
% level 3 suffices for each case.
%
% Example usage:
%
% (1)
%      x = rand(100,1);
%      y = rand(100,1);
%      z = rand(100,1);
%      [rotmat,cornerpoints,volume,surface] = minboundbox(x,y,z);
%      plot3(x,y,z,'b.');hold on;plotminbox(cornerpoints,'r');
%
% (2)
%      x=[1,0,0.1,1];y=[0,1,0.1,1];z=[0,0,0.9,1];
%      [nerd,cornerpoints1,nerd,nerd] = minboundbox(x,y,z,'v',1);
%      [nerd,cornerpoints2,nerd,nerd] = minboundbox(x,y,z,'v',2);
%      [nerd,cornerpoints3,nerd,nerd] = minboundbox(x,y,z,'v',3);
%      plot3(x,y,z,'bo','LineWidth',5);hold on;
%      plotminbox(cornerpoints1,'b');hold on;
%      plotminbox(cornerpoints2,'m');hold on;
%      plotminbox(cornerpoints3,'r');axis equal;grid on;hold off;
%
%
% See also: minboundcircle, minboundtri, minboundsphere, minboundrect
%
%
% Author: Johannes Korsawe
% E-mail: johannes.korsawe@volkswagen.de
% Release: 1.0
% Release date: 09/01/2008

% default for metric
if (nargin<4) || isempty(metric)
  metric = 'v';
elseif ~ischar(metric)
  error 'metric must be a character flag if it is supplied.'
else
  % check for 'v', 's' or 'e'
  metric = lower(metric(:)');
  ind = strmatch(metric,{'volume','surface','edges'});
  if isempty(ind)
    error 'metric does not match either ''volume'', ''surface'' or ''edges''.'
  end
  % just keep the first letter.
  metric = metric(1);
end

% default for level
if (nargin<5 || isempty(level)),
    level=4;
elseif ~isnumeric(level) || (level~=1 && level~=2 && level~=3 && level~=4),
        error 'metric does not match either ''1'', ''2'', ''3'' or ''3''.'
end

% preprocess data
x = x(:);
y = y(:);
z = z(:);

% not many error checks to worry about
n1 = length(x);n2 = length(y);n3 = length(z);
if n1 ~= n2 || n1 ~= n3 || n2 ~= n3,
  error 'x, y and z must be the same sizes'
end

% start out with the convex hull of the points to
% reduce the problem dramatically. Note that any
% points in the interior of the convex hull are
% never needed, so we drop them.

try
 K = convhulln([x,y,z],{'Qt'});  % 'Pp' will silence the warnings

  % exclude those points inside the hull as not relevant
  % also sorts the points into their convex hull as a
  % closed polygon

 Ki = unique(K(:));
 [tf,loc] = ismember(K(:),Ki);K = reshape(loc,size(K));
 x = x(Ki);
 y = y(Ki);
 z = z(Ki);
 n1 =length(x);

catch
    error 'The number and/or distribution of given points does not allow the construction of a convex hull.'
end

% now we must find the bounding box of those points that remain

% get the angle of each face of the hull polygon.
% calculate local coordinate system
% x,y and z values of faces' cornerpoints
fx = [x(K(:,1)),x(K(:,2)),x(K(:,3))];
fy = [y(K(:,1)),y(K(:,2)),y(K(:,3))];
fz = [z(K(:,1)),z(K(:,2)),z(K(:,3))];
% faces' edges
v1 = [fx(:,2)-fx(:,1),fy(:,2)-fy(:,1),fz(:,2)-fz(:,1)];
v1 = v1./(repmat(sqrt(sum(v1.^2,2)),1,3));
v2 = [fx(:,3)-fx(:,1),fy(:,3)-fy(:,1),fz(:,3)-fz(:,1)];
v2 = v2-repmat(dot(v1,v2,2),1,3).*v1;
v2 = v2./(repmat(sqrt(sum(v2.^2,2)),1,3));
% faces' normals
nv = cross(v1,v2,2);
% faces' orientations measured in Euler angles
[alpha,beta,gamma]=euler123(v1,v2,nv);

nang = size(alpha,1);
d = inf;        % this will be the minimal value
rotmat=[];minmax=[];
xyz = [x,y,z];
if level==1 || level==3,    % check each face of the hull
    for i = 1:nang,
      % check current orientation
      % receive minimal value, rotation matrix and dimensions of the enclosing box
      [d, rotmat, minmax] = checkbox(alpha(i),beta(i),gamma(i),xyz,metric,d,rotmat,minmax);
    end
end

if level>1,
    e = edgelist(K);ne = size(e,1); % get a sorted list of edges
end

if level==2 || level==4,    % check each set of two edges of the convex hull (which obviously includes case '1' above)
    for i = 1:ne,   % go through edge list
        va = xyz(e(i,1),:)-xyz(e(i,2),:);
        va = va/norm(va);
        for j = i+1:ne, % go through remaining edge list
            vb = xyz(e(j,1),:)-xyz(e(j,2),:);
            vb = vb - dot(va,vb)*va; % orthogonaylize second edge wrt first
            nv = cross(va,vb);  % normal to plane formed by the two edges
            if sum(abs(nv))>0,
                vb = vb/norm(vb);nv = nv/norm(nv);
                [alp,bet,gam]=euler123(va,vb,nv);
                [d, rotmat, minmax] = checkbox(alp,bet,gam,xyz,metric,d,rotmat,minmax);
            end
        end
    end
end

if level==3 || level==4,    % check edge as parallel to one edge of the bounding box
    for i = 1:ne, % go through edge list
        % calculate rhs with edge as one of the axes
        va = xyz(e(i,1),:)-xyz(e(i,2),:);
        vb = [va(2),-va(1),0];if sum(abs(vb))==0,vb = [va(3),0,-va(1)];end
        va = va/norm(va);vb = vb/norm(vb);
        nv = cross(va,vb);
        % check all combinations of possible rhs
        [alp,bet,gam]=euler123(va,vb,nv);
        [d, rotmat, minmax] = checkbox(alp,bet,gam,xyz,metric,d,rotmat,minmax);
        [alp,bet,gam]=euler123(vb,nv,va);
        [d, rotmat, minmax] = checkbox(alp,bet,gam,xyz,metric,d,rotmat,minmax);
        [alp,bet,gam]=euler123(nv,va,vb);
        [d, rotmat, minmax] = checkbox(alp,bet,gam,xyz,metric,d,rotmat,minmax);
    end
end

% get the output values
h = [minmax(1,1),minmax(1,2),minmax(1,3);    % xmin,ymin,zmin
     minmax(2,1),minmax(1,2),minmax(1,3);    % xmax,ymin,zmin
     minmax(2,1),minmax(2,2),minmax(1,3);    % xmax,ymax,zmin
     minmax(1,1),minmax(2,2),minmax(1,3);    % xmin,ymax,zmin
     minmax(1,1),minmax(1,2),minmax(2,3);    % xmin,ymin,zmax
     minmax(2,1),minmax(1,2),minmax(2,3);    % xmax,ymin,zmax
     minmax(2,1),minmax(2,2),minmax(2,3);    % xmax,ymax,zmax
     minmax(1,1),minmax(2,2),minmax(2,3);    % xmin,ymax,zmax
     ];

cornerpoints = h*inv(rotmat); % minimal boxes' cornerpoints
h = minmax(2,:)-minmax(1,:);
volume = h(1)*h(2)*h(3);
surface = 2*(h(1)*h(2)+h(2)*h(3)+h(3)*h(1));
edgelength = 4*sum(h);

% all done

end % mainline end

function [alpha,beta,gamma]=euler123(v1,v2,nv)
% calculate Euler angles for the x, y', z'' Euler sequence
% see also
% http://en.wikipedia.org/wiki/Euler_angles
% http://de.wikipedia.org/wiki/Eulersche_Winkel

% --> beta
beta = asin(sign(nv(:,1)).*min(1,abs(nv(:,1))));
% --> alpha
alpha=0*beta;
i1 = find(nv(:,1)==1);i2 = setdiff(1:size(nv,1),i1);
if ~isempty(i1),
    alpha(i1) = asin(sign(v2(i1,3)).*min(1,abs(v2(i1,3))));
end
if ~isempty(i2),
    alpha(i2) = acos(sign(nv(i2,3)./cos(beta(i2))).*min(1,abs(nv(i2,3)./cos(beta(i2)))));
    i3 = find(sign(nv(i2,2)) ~= sign(-sin(alpha(i2)).*cos(beta(i2))));
    alpha(i2(i3)) = -alpha(i2(i3));
end
% --> gamma
gamma = 0*alpha;
if ~isempty(i2),
    singamma = -v2(i2,1)./cos(beta(i2));
    i21 = find(v1(i2,1)>=0);i22=setdiff(1:length(i2),i21);
    gamma(i2(i21)) = asin(sign(singamma(i21)).*min(1,abs(singamma(i21))));
    gamma(i2(i22)) = -pi-asin(sign(singamma(i22)).*min(1,abs(singamma(i22))));
end

end % function euler123

function [d, rotmat, minmax] = checkbox(alpha,beta,gamma,xyz,metric,d,rotmat,minmax)

% will need a 3x3 rotation matrix through (Euler X, Y', Z'')-angles alpha, beta and gamma
Rmat = @(alpha,beta,gamma) [cos(beta)*cos(gamma) -cos(beta)*sin(gamma) sin(beta)
                            sin(alpha)*sin(beta)*cos(gamma)+cos(alpha)*sin(gamma) -sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma) -sin(alpha)*cos(beta)
                            -cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma) cos(alpha)*sin(beta)*sin(gamma)+sin(alpha)*cos(gamma) cos(alpha)*cos(beta)];

rot = Rmat(alpha,beta,gamma);
xyz_i = xyz*rot;                      % now the actual face is in the x-y plane

x_i = xyz_i(:,1);y_i = xyz_i(:,2);          % .. so take only the x and y values
rot2 = minrect(x_i,y_i,metric);             % find the optimal rotation around z-axis
rot = rot*[[rot2,[0;0]];[0,0,1]];           % combine that with the formar rotation
xyz_i = xyz*rot;                            % now again the xyz_i values, but in optimal axisparallel shape

xyzmin = min(xyz_i,[],1);
xyzmax = max(xyz_i,[],1);
h = xyzmax-xyzmin;

if metric == 'v',       % smallest volume
    d_i = h(1)*h(2)*h(3);
elseif metric == 's',   % smallest surface
    d_i = h(1)*h(2)+h(2)*h(3)+h(3)*h(1);
else,                   % smallest sum of edges
    d_i = sum(h);
end

if d_i < d,
    d = d_i;
    rotmat = rot;
    minmax = [xyzmin;xyzmax];
end

end % function checkbox

function rot = minrect(x,y,metric)
% see comments from minboundrect of John d'Errico
% i am only interested in the additional rotation matrix around [0,0,1]
edges = convhull(x,y,{'Qt'});
x = x(edges);y = y(edges);
ind = 1:length(x)-1;
Rmat = @(theta) [cos(theta) sin(theta);-sin(theta) cos(theta)];
edgeangles = atan2(y(ind+1) - y(ind),x(ind+1) - x(ind));edgeangles = unique(mod(edgeangles,pi/2));
nang = length(edgeangles);
area = inf;perimeter = inf;met = inf;xy = [x,y];
for i = 1:nang
  rot_i = Rmat(-edgeangles(i));xyr = xy*rot_i;
  xymin = min(xyr,[],1);xymax = max(xyr,[],1);
  A_i = prod(xymax - xymin);P_i = 2*sum(xymax-xymin);
  if metric=='v', M_i = A_i;else, M_i = P_i;end
  if M_i<met, rot = rot_i;met = M_i;end
end

end % function minrect

function e = edgelist(K)
% generate an edgelist from the convex body K
e = [K(:,1),K(:,2)];e = [e;K(:,2),K(:,3)];e = [e;K(:,1),K(:,3)];
e = [min(e,[],2),max(e,[],2)];ne = size(e,1);
e = [e,e(:,2)+ne*e(:,1)];
[nerd,I] = sort(e(:,3));
e = e(I(1:2:end),:);

end % edgelist


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 CavitySystemID(DataFileName)

% building the Koopman linear system for cavity flow
% from the data in the file named "DataFileName"
disp('loading EDMD data ...')

% reminder
if ~strcmp(DataFileName,'Cavity_data_4EDMD_0.mat')
        warning(['if yu are using your own data', ...
        'make sure to remove the snapshots that contain NaN or Inf']);
end

load(DataFileName)

% the results are better if we center the data around point close to action
% in the state space
SUBTRACT_MEAN = 1;
if(SUBTRACT_MEAN)
    load('CavityStateLibrary.mat','FixedPoint_Re10k')
    x_mean = FixedPoint_Re10k;
    X = bsxfun(@minus,X,x_mean);
    Y = bsxfun(@minus,Y,x_mean);
end

% the following sensor locations generate the plots in the paper
load('CavitySensorLocations.mat','pindex')

% construct models for various number (k) of spatial measurements
for k = [2,5,10,40,50,100]
    tic

    Grid= CavityGridOperators( SimPar.N );
    Grid.Interior_index = setdiff(1:length(Grid.xx),Grid.Walls);

    sampind = pindex(1:k);
    % sampind is the index of the measurement point on the Chebyshev grid
    % feel free to replace sampind with some random numbers between 1 and
    % 47^2,

    CollectOutput = @(x) x(sampind,:);

    % location in the cavity domain
    figure(311)
    plot(CollectOutput(Grid.xx),CollectOutput(Grid.yy),'x')
    axis square
    axis([-1,1,-1,1])
    title(['location of sensors in cavity, k=',num2str(k)])
    drawnow
    XX = CollectOutput(X);
    YY = CollectOutput(Y);

    % embed the data from each trajectory
    nd = 5; % delay order
    tic
    Xtilde =[]; Ytilde=[]; Utilde=[];

    SimLength=unique(SimLengthVar);
    if length(SimLength)>1
        error('trajectories of different lengtnhs .. adopt')
    end
    for i=1:length(SimLengthVar)

        % consider adopting to different trajectory lengths

        TrajIndex = (i-1)*SimLength + (1:SimLength);    % indexes of each trajectory

        % embed the data
        % this gives upside down - think if ordering matters
        Xe = DelayEmbed(XX(:,TrajIndex) , nd    );
        Ye = DelayEmbed(YY(:,TrajIndex) , nd    );
        Ue = DelayEmbed( U(:,TrajIndex) , nd - 1);

        % add the delay-embedded input as an observable
        Xtilde = [Xtilde,[Xe;Ue(:,1:end-1)]];
        Ytilde = [Ytilde,[Ye;Ue(:,2:end  )]];

        UIndex = (i-1)*SimLength + (nd:SimLength);    % indexes of each trajectory
        Utilde = [Utilde, U(:,UIndex) ];

    end

    % add l-2 norm of recent output and constant as extra observables
    KE_embed = @(y) sum(abs(y((nd-1)*k+1:nd*k,:)).^2,1);

    % now lift the data
    X_lift = [Xtilde;KE_embed(Xtilde);ones(1,size(Xtilde,2))];
    Y_lift = [Ytilde;KE_embed(Ytilde);ones(1,size(Ytilde,2))];
    U_lift = Utilde;

    % compute the Koopman linear system
    % Via EDMD in the normal form
    W = [X_lift;U_lift]*[X_lift;U_lift]';
    V = Y_lift*[X_lift;U_lift]';
    M = V*pinv(W);

    Nlift = size(X_lift,1);
    A = M(:,1:Nlift);
    B = M(:,Nlift+1:end);
    C = zeros(k,size(A,1));
    C(:,(nd-1)*k+1:nd*k)=eye(k);

    % define a lift function to be used in control:
    % first a simple dely-embed that lines up the last nd columns of given
    % data matrix
    Delay_Embed = @(X,nd) reshape(X(:,end-nd+1:end),nd*size(X,1),1);

    % then a function that forms zeta given the state and input matrices
    CreateZeta = @(X,U) [Delay_Embed(CollectOutput(X(:,end-nd+1:end)),nd)...
                            ;Delay_Embed(U(:,1:end-1),nd-1)];

    % then the second lifting
    % the last function gets the actual state and input and returns the
    % Koopman linear state
    LiftFun = @(X,U) [CreateZeta(X,U); sum(CollectOutput(X(:,end)).^2);1];

    modelfilename = ['KoopmanLinSys_Re13_k',num2str(k),'_random'];
    save(modelfilename,'A','B','C','nd','CollectOutput','KE_embed','x_mean','LiftFun','CreateZeta','Delay_Embed')

    fprintf('Koopman model of cavity created using %d measurements \n',k)
    toc
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


%% SOS Testing
%% LP Solve Example SOS1
clc
f = [-1 -1 -3 -2 -2]';
A = [-1 -1 1 1 0;
     1 0 1 -3 0];
b = [30;30];
ub = [40;1;inf;inf;1];

sostype = '12';
sosind = {[1:2]' [3:5]'};
soswt = {[1:2]' [3:5]'};

% [x,fval,ef,stat] = cplexmilp(f,A,b,[],[],sos,sosind,soswt,[],ub)

% SOS 1 OPTI Version
opts = optiset('solver','cbc');

prob = optiprob('f',f,'ineq',A,b,'ub',ub,'sos',sostype,sosind,soswt,'options',opts)

sos = [];
sos.type = sostype;
sos.index = sosind;
sos.weight = soswt;
prob2 = optiprob('f',f,'ineq',A,b,'ub',ub,'sos',sos,'options',opts)

Opt = opti(prob,opts)

[x,fval,ef,stat] = solve(Opt)

%% LP Solve Example SOS2
clc
f = [-1 -1 -3 -2 -2]';
A = sparse([-1 -1 1 1 0;
             1 0 1 -3 0]);
b = [30;30];

lb = zeros(5,1);
ub = [40;1;inf;inf;1];

sos = '1';
sosind = (1:5)';
soswt = (1:5)';

% [x,fval,ef,stat] = cplexmilp(f,A,b,[],[],sos,sosind,soswt,lb,ub,'IIIII');
% x
% stat

% SOS 2 OPTI Version
opts = optiset('solver','cbc');

prob = optiprob('f',f,'ineq',A,b,'bounds',lb,ub,'sos',sos,sosind,soswt,'options',opts)

Opt = opti(prob,opts)

[x,fval,ef,stat] = solve(Opt)

% [x,fval,ef,stat] = solve(Opt);
% x
% fval
% stat

%%
clc
% Objective (f'x)
f = [-1 -1 -3 -2 -2]';

% Row Constraints (rl <= A*x <= ru)
A = sparse([-1 -1  1  1  0;     %sparse A
             1  0  1 -3  0]);
rl = [-Inf;-Inf];
ru = [30;30];

% Bounds (lb <= x <= ub)
lb = [0;0;0;0;0];
ub = [40;1;Inf;Inf;1];

% SOS
sos_type = '1';
sos_index = [1 2 3 4 5]';
sos_weight = [1 2 3 4 5]';

% Setup Options
opts = optiset('solver','cbc'); %CBC is a SOS constraint solver

% Build OPTI Object
Opt = opti('f',f,'lin',A,rl,ru,'bounds',lb,ub,'sos',sos_type,sos_index,...
           sos_weight,'options',opts)

% Solve Problem
[x,fval] = solve(Opt)

%%
% SOS Structure
sos = [];
sos.type = '12'; %each character represents a SOS set
sos.index = {[1 2]' [3:5]'};
sos.weight = {[1 2]' [1:3]'};

% Build OPTI Object
Opt = opti('f',f,'lin',A,rl,ru,'bounds',lb,ub,'sos',sos,'options',opts)

% Solve Problem
[x,fval] = solve(Opt)


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 fx2 = p18_fx2 ( x )

%*****************************************************************************80
%
%% P18_FX2 evaluates the second derivative of the function for problem 18.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(*), the point at which F is to be evaluated.
%
%    Output, real FX2(*), the value of the second derivative at X.
%
  fx2 = 10.0^14 * ( ...
         42 * x.^5 ...
      - 210 * x.^4 ...
      + 420 * x.^3 ...
      - 420 * x.^2 ...
      + 210 * x    ...
      -  42 );

  return
end


% test for sparsity-based deblurring

path(path, 'toolbox/');

n = 128;
name = 'lena';
sigma = 0.04; % noise level

% load the image
M0 = rescale( crop( load_image(name), n), 0.05 );

% options for the wavelet transform
Jmin = 4;
options.Jmin = Jmin;
options.wavelet_type = 'biorthogonal_swapped';
options.wavelet_vm = 4;

% width of the filter
options.eta = 3;
% observation
Y = perform_blurring( M0, options.eta ) + sigma*randn(n);

% do inversion by iterative thresholding
lambda_min = 0;
lambda_max = 0.05;
nbr = 10;
lambda_list = linspace(lambda_max, lambda_min, nbr);

options.niter = 12; % iteration for inversion
options.mu = 1;
X = zeros(n^2,1); M = {}; perr = [];
for lambda = lambda_list
    options.lambda = lambda;
    options.X = X;
    X = perform_iterative_thresholding( @callback_blurring, Y(:), options );
    M{end+1} = perform_wavelet_transform(reshape(X,n,n), Jmin, -1, options);
    perr(end+1) = psnr(M0, M{end});
end

% plot error along regularization path
clf;
plot(lambda_list,perr);
xlabel('\lambda'); ylabel('PSNR');

% find best regularization lambda for inversion (this is cheating ...)
[pval,i] = max(perr);
clf;
imageplot(M0, 'Original', 1,3,1);
imageplot(Y, ['Observed, PSNR=' num2str(psnr(Y,M0))], 1,3,2);
imageplot(M{i}, ['Recovered, PSNR=' num2str(pval)], 1,3,3);


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 alpha = circ_ang2rad(alpha)

% alpha = circ_ang2rad(alpha)
%   converts values in degree to radians
%
% Circular Statistics Toolbox for Matlab

% By Philipp Berens, 2009
% berens@tuebingen.mpg.de - www.kyb.mpg.de/~berens/circStat.html

alpha = alpha * pi /180;


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 Animation(ivp, duration, fps, movie)
%--------------------------------------------------------------------------
% To Animate the Bead on a rotating hoop, Equation is solved by MATLAB ode45, and
% running a loop over the time step; for every time step the position
% coordinates of the bead are updated. Phase plane plot and time Vs.
% displacement plot is also plotted
%--------------------------------------------------------------------------
nframes=duration*fps;           % Number of Frames
% Solving the Equation using ode45
sol=ode45(@Equation,[0 duration], ivp);
t = linspace(0,duration,nframes);
y = deval(sol,t);
% Position and Angular velocity of the bead
thita = y(1,:)';
dthita = y(2,:)';
R = ivp(5);                     % Radius of the Hoop
w0 = ivp(7) ;                   % Grequency of rotation of the hoop/ring
%
% Figure starts
fh = figure ;
set(fh,'name','Bead on Rotating Hoop','numbertitle','off','color', 'k','menubar','none') ;
% Plot for the Hoop (Ring/Circle)
phi = linspace(0,2*pi,nframes) ;
c = [ 0 0 0] ;                  % Center of the hoop
u = [1 0 0] ;                   % Normal vectors lying on the plane of circle
v = [0 0 1] ;
px = c(1)+R*cos(phi).*u(1)+R*sin(phi).*v(1) ;
py = c(2)+R*cos(phi).*u(2)+R*sin(phi).*v(2) ;
pz = c(3)+R*cos(phi).*u(3)+R*sin(phi).*v(3) ;
hoop = plot3(px,py,pz,'Color','r','Linewidth',3) ;
% Bead position
beadX = R*sin(thita).*cos(w0*t') ;
beadY = R*sin(thita).*sin(w0*t') ;
beadZ = -R*cos(thita) ;
hold on ;
bead =  plot3(beadX(1),beadY(1),beadZ(1),'MarkerSize',50,'Marker','.','Color','b');
% Trajectory of the bead on rotating hoop
path = plot3(beadX(1),beadY(1),beadZ(1),'Color','w','Linewidth',0.5) ;
% Figure Settings
title('Bead on a Rotating Hoop','Color','w','Fontsize',10);
range = 1.3*R;
campos([0 R 0]) ;
axis([-range range -range range -range range]) ;
axis square ;
axis off ;
rotate3d ;
stop = uicontrol('style','toggle','string','stop','background','white');
%
% Animation starts
for i=1:length(t)-1
 % Animation Plot
    if (ishandle(fh)==1)
        T = [cos(w0*t(i))  -sin(w0*t(i)) 0 ;
             sin(w0*t(i))   cos(w0*t(i)) 0 ;
             0          0       1] ;        % Rotation Matrix
       pn = T*[px;py;pz] ;                  % New Position of the Hoop
       pn = pn' ;
       set(hoop,'XData',pn(:,1),'YData',pn(:,2),'ZData',pn(:,3)) ;
       set(bead,'XData',beadX(i),'YData',beadY(i),'ZData',beadZ(i)) ;
      % drawnow;
        % Trajectory of the Bead
       PP(i,:) = [beadX(i,1) beadY(i,1) beadZ(i,1)];
       set(path,'XData',PP(:,1),'YData',PP(:,2),'ZData',PP(:,3));
            if get(stop,'value')==0
                drawnow;
            elseif get(stop,'value')==1
                break
            end
    end
    F(i) = getframe(fh) ;
    % Pause for length of time step
    if movie == false
            pause(t(i+1)-t(i));
    end
end
if movie == true
        movie2avi(F,'BeadOnRotatingHoop.avi','compression','Cinepak','fps',fps)
end
set(stop,'style','pushbutton','string','close','callback','close(gcf)');


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 w = ncc_compute_weights ( n )

%*****************************************************************************80
%
%% NCC_COMPUTE_WEIGHTS: weights of a Newton-Cotes Closed quadrature rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order.
%
%    Output, real W(N), the weights.
%
  x_min = -1.0;
  x_max =  1.0;

  if ( n == 1 )

    w(1) = x_max - x_min;

  else

    x = linspace ( x_min, x_max, n );

    w = nc_compute ( n, x_min, x_max, x );

  end

  return
end


function r8lib_test ( )

%*****************************************************************************80
%
%% R8LIB_TEST tests the R8LIB library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 May 2015
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8LIB_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the R8LIB library.\n' );

  i4int_to_r8int_test ( );

  perm0_check_test ( );
  perm0_uniform_test ( );

  perm1_check_test ( );
  perm1_uniform_test ( );

  r8_abs_test ( );
  r8_acos_test ( );
  r8_acosh_test ( );
  r8_asinh_test ( );
  r8_atan_test ( );
  r8_atanh_test ( );
  r8_big_test ( );
  r8_cas_test ( );
  r8_ceiling_test ( );
  r8_choose_test ( );
  r8_cosd_test ( );
  r8_cotd_test ( );
  r8_cscd_test ( );
  r8_cube_root_test ( );
  r8_diff_test ( );
  r8_digit_test ( );
  r8_e_test ( );
  r8_epsilon_test ( );
  r8_epsilon_compute_test ( );
  r8_factorial_test ( );
  r8_factorial2_test ( );
  r8_fall_test ( );
  r8_fractional_test ( );
  r8_gamma_test ( );
  r8_gamma_log_test ( );
  r8_huge_test ( );
  r8_log_2_test ( );
  r8_log_b_test ( );
  r8_mant_test ( );
  r8_max_test ( );
  r8_min_test ( );
  r8_mod_test ( );
  r8_modp_test ( );
  r8_mop_test ( );
  r8_nint_test ( );
  r8_normal_01_test ( );
  r8_pi_test ( );
  r8_power_test ( );
  r8_power_fast_test ( );
  r8_rise_test ( );
  r8_round2_test ( );
  r8_roundb_test ( );
  r8_roundx_test ( );
  r8_secd_test ( );
  r8_sign_test ( );
  r8_sign3_test ( );
  r8_sind_test ( );
  r8_swap_test ( );
  r8_swap3_test ( );
  r8_tand_test ( );
  r8_to_r8_discrete_test ( );
  r8_to_i4_test ( );
  r8_uniform_ab_test ( );
  r8_uniform_01_test ( );
  r8_walsh_1d_test ( );
  r8_wrap_test ( );

  r82col_print_part_test ( );

  r82poly2_type_test ( );

  r82row_order_type_test ( );
  r82row_part_quick_a_test ( );
  r82row_print_part_test ( );
  r82row_sort_heap_index_a_test ( );
  r82row_sort_quick_a_test ( );

  r83col_print_part_test ( );

  r83row_print_part_test ( );

  r8block_expand_linear_test ( );
  r8block_print_test ( );

  r8col_find_test ( );
  r8col_insert_test ( );
  r8col_max_test ( );
  r8col_mean_test ( );
  r8col_min_test ( );
  r8col_permute_test ( );
  r8col_sort_heap_a_test ( );
  r8col_sort_heap_index_a_test ( );
  r8col_sort_quick_a_test ( );
  r8col_sorted_tol_unique_test ( );
  r8col_sorted_tol_unique_count_test ( );
  r8col_sorted_tol_undex_test ( );
  r8col_sortr_a_test ( );
  r8col_sum_test ( );
  r8col_swap_test ( );
  r8col_to_r8vec_test ( );
  r8col_tol_undex_test ( );
  r8col_undex_test ( );
  r8col_unique_count_test ( );
  r8col_variance_test ( );

  r8int_to_i4int_test ( );

  r8r8vec_index_insert_unique_test ( );

  r8r8r8vec_index_insert_unique_test ( );

  r8mat_cholesky_inverse_test ( );
  r8mat_cholesky_solve_test ( );
  r8mat_cholesky_solve_upper_test ( );
  r8mat_det_2d_test ( );
  r8mat_det_3d_test ( );
  r8mat_det_4d_test ( );
  r8mat_det_5d_test ( );
  r8mat_expand_linear_test ( );
  r8mat_expand_linear2_test ( );
  r8mat_fs_test ( );
  r8mat_fss_test ( );
  r8mat_givens_post_test ( );
  r8mat_givens_pre_test ( );
  r8mat_hess_test ( );
  r8mat_house_axh_test ( );
  r8mat_house_form_test ( );
  r8mat_house_post_test ( );
  r8mat_house_pre_test ( );
  r8mat_indicator_test ( );
  r8mat_inverse_2d_test ( );
  r8mat_inverse_3d_test ( );
  r8mat_inverse_4d_test ( );
  r8mat_jac_test ( );
  r8mat_kronecker_test ( );
  r8mat_l_inverse_test ( );
  r8mat_l_print_test ( );
  r8mat_l1_inverse_test ( );
  r8mat_lu_test ( );
  r8mat_max_test ( );
  r8mat_max_index_test ( );
  r8mat_min_test ( );
  r8mat_min_index_test ( );
  r8mat_maxcol_minrow_test ( );
  r8mat_maxrow_mincol_test ( );
  r8mat_mincol_maxrow_test ( );
  r8mat_minrow_maxcol_test ( );
  r8mat_mm_test ( );
  r8mat_mv_test ( );
  r8mat_nint_test ( );
  r8mat_nonzeros_test ( );
  r8mat_norm_fro_test ( );
  r8mat_norm_l1_test ( );
  r8mat_nullspace_test ( );
  r8mat_nullspace_size_test ( );
  r8mat_orth_uniform_test ( );
  r8mat_plot_test ( );
  r8mat_power_method_test ( );
  r8mat_print_test ( );
  r8mat_print_some_test ( );
  r8mat_ref_test ( );
  r8mat_rref_test ( );
  r8mat_solve_test ( );
  r8mat_solve_2d_test ( );
  r8mat_solve_3d_test ( );
  r8mat_solve2_test ( );
  r8mat_sub_test ( );
  r8mat_symm_jacobi_test ( );
  r8mat_to_r8plu_test ( );
  r8mat_trace_test ( );
  r8mat_transpose_print_test ( );
  r8mat_transpose_print_test ( );
  r8mat_u_inverse_test ( );
  r8mat_u1_inverse_test ( );
  r8mat_uniform_ab_test ( );

  r8plu_det_test ( );
  r8plu_inverse_test ( );
  r8plu_mul_test ( );
  r8plu_sol_test ( );
  r8plu_to_r8mat_test ( );

  r8poly_degree_test ( );
  r8poly_deriv_test ( );
  r8poly_lagrange_coef_test ( );
  r8poly_lagrange_0_test ( );
  r8poly_lagrange_1_test ( );
  r8poly_lagrange_2_test ( );
  r8poly_lagrange_factor_test ( );
  r8poly_lagrange_val_test ( );
  r8poly_print_test ( );
  r8poly_value_horner_test ( );
  r8poly_values_horner_test ( );

  r8poly2_ex_test ( );
  r8poly2_ex2_test ( );
  r8poly2_val_test ( );
  r8poly2_val2_test ( );
  r8poly2_root_test ( );

  r8poly3_root_test ( );

  r8poly4_root_test ( );

  r8row_max_test ( );
  r8row_mean_test ( );
  r8row_min_test ( );
  r8row_sum_test ( );
  r8row_swap_test ( );
  r8row_to_r8vec_test ( );
  r8row_variance_test ( );

  r8slmat_print_test ( );

  r8vec_amax_test ( );
  r8vec_amin_test ( );
  r8vec_bracket_test ( );
  r8vec_bracket2_test ( );
  r8vec_bracket3_test ( );
  r8vec_bracket5_test ( );
  r8vec_chebyspace_test ( );
  r8vec_concatenate_test ( );
  r8vec_convolution_test ( );
  r8vec_convolution_circ_test ( );
  r8vec_dif_test ( );
  r8vec_direct_product_test ( );
  r8vec_direct_product2_test ( );
  r8vec_even_test ( );
  r8vec_even2_test ( );
  r8vec_even3_test ( );
  r8vec_expand_linear_test ( );
  r8vec_frac_test ( );
  r8vec_histogram_test ( );
  r8vec_house_column_test ( );
  r8vec_index_delete_all_test ( );
  r8vec_index_delete_dupes_test ( );
  r8vec_index_delete_one_test ( );
  r8vec_index_insert_test ( );
  r8vec_index_insert_unique_test ( );
  r8vec_index_order_test ( );
  r8vec_index_search_test ( );
  r8vec_index_sorted_range_test ( );
  r8vec_indexed_heap_d_test ( );
  r8vec_indexed_heap_d_extract_test ( );
  r8vec_indexed_heap_d_insert_test ( );
  r8vec_indexed_heap_d_max_test ( );
  r8vec_indicator0_test ( );
  r8vec_indicator1_test ( );
  r8vec_legendre_test ( );
  r8vec_linspace_test ( );
  r8vec_max_test ( );
  r8vec_max_index_test ( );
  r8vec_mean_test ( );
  r8vec_median_test ( );
  r8vec_midspace_test ( );
  r8vec_min_test ( );
  r8vec_min_index_test ( );
  r8vec_nint_test ( );
  r8vec_norm_l0_test ( );
  r8vec_norm_l1_test ( );
  r8vec_norm_l2_test ( );
  r8vec_norm_li_test ( );
  r8vec_normal_01_test ( );
  r8vec_normalize_l1_test ( );
  r8vec_order_type_test ( );
  r8vec_permute_test ( );
  r8vec_permute_uniform_test ( );
  r8vec_polarize_test ( );
  r8vec_print_test ( );
  r8vec_rotate_test ( );
  r8vec_reverse_test ( );
  r8vec_search_binary_a_test ( );
  r8vec_sort_bubble_a_test ( );
  r8vec_sort_heap_a_test ( );
  r8vec_sort_heap_d_test ( );
  r8vec_sort_heap_index_a_test ( );
  r8vec_sort_heap_index_d_test ( );
  r8vec_sort_heap_mask_a_test ( );
  r8vec_sort_insert_a_test ( );
  r8vec_sort_insert_index_a_test ( );
  r8vec_sort_quick_a_test ( );
  r8vec_sorted_merge_a_test ( );
  r8vec_sorted_nearest_test ( );
  r8vec_sorted_range_test ( );
  r8vec_sorted_split_test ( );
  r8vec_sorted_undex_test ( );
  r8vec_sorted_unique_test ( );
  r8vec_sorted_unique_count_test ( );
  r8vec_sorted_unique_hist_test ( );
  r8vec_split_test ( );
  r8vec_transpose_print_test ( );
  r8vec_undex_test ( );
  r8vec_uniform_01_test ( );
  r8vec_uniform_ab_test ( );
  r8vec_variance_test ( );

  r8vec2_sort_a_test ( );
  r8vec2_sort_d_test ( );
  r8vec2_sort_heap_index_a_test ( );
  r8vec2_sorted_unique_test ( );
  r8vec2_sorted_unique_index_test ( );
  r8vec2_sum_max_index_test ( );

  roots_to_r8poly_test ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8LIB_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
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));


clear();

numRows     = 3;
numVectors  = 6;

%% Load / Generate Data

mW = randn(numRows, numRows);
mW = (mW.' * mW) + (0.00005 + eye(numRows));

mXX = randn(numRows, numVectors); %<! The set of {x}_{i} (Each column)

mX = zeros(numVectors, numRows * numRows);
for ii = 1:numVectors
    mX(ii, :) = kron(mXX(:, ii).', mXX(:, ii).');
end

vY = randn(numVectors, 1);

objVal = 0;

for ii = 1:numVectors
    objVal = objVal + (((mXX(:, ii).' * mW * mXX(:, ii)) - vY(ii)) ^ 2);
end

objVal
sum((mX * mW(:) - vY) .^ 2)

% ii = 8;
% mXX(:, ii).' * mW * mXX(:, ii)
% mX(ii, :) * mW(:)


function [y,w] = idctn(y,DIM,w)

%IDCTN N-D inverse discrete cosine transform.
%   X = IDCTN(Y) inverts the N-D DCT transform, returning the original
%   array if Y was obtained using Y = DCTN(X).
%
%   IDCTN(X,DIM) applies the IDCTN operation across the dimension DIM.
%
%   Class Support
%   -------------
%   Input array can be numeric or logical. The returned array is of class
%   double.
%
%   Reference
%   ---------
%   Narasimha M. et al, On the computation of the discrete cosine
%   transform, IEEE Trans Comm, 26, 6, 1978, pp 934-936.
%
%   Example
%   -------
%       RGB = imread('autumn.tif');
%       I = rgb2gray(RGB);
%       J = dctn(I);
%       imshow(log(abs(J)),[]), colormap(jet), colorbar
%
%   The commands below set values less than magnitude 10 in the DCT matrix
%   to zero, then reconstruct the image using the inverse DCT.
%
%       J(abs(J)<10) = 0;
%       K = idctn(J);
%       figure, imshow(I)
%       figure, imshow(K,[0 255])
%
%   See also DCTN, IDSTN, IDCT, IDCT2.
%
%   -- Damien Garcia -- 2009/04, revised 2009/11
%   website: <a
%   href="matlab:web('http://www.biomecardio.com')">www.BiomeCardio.com</a>

% ----------
%   [Y,W] = IDCTN(X,DIM,W) uses and returns the weights which are used by
%   the program. If IDCTN is required for several large arrays of same
%   size, the weights can be reused to make the algorithm faster. A typical
%   syntax is the following:
%      w = [];
%      for k = 1:10
%          [y{k},w] = idctn(x{k},[],w);
%      end
%   The weights (w) are calculated during the first call of IDCTN then
%   reused in the next calls.
% ----------

error(nargchk(1,3,nargin))

y = double(y);
sizy = size(y);

% Test DIM argument
if ~exist('DIM','var'), DIM = []; end
assert(~isempty(DIM) || ~isscalar(DIM),...
    'DIM must be a scalar or an empty array')
assert(isempty(DIM) || DIM==round(DIM) && DIM>0,...
    'Dimension argument must be a positive integer scalar within indexing range.')

% If DIM is empty, a DCT is performed across each dimension

if isempty(DIM), y = squeeze(y); end % Working across singleton dimensions is useless
dimy = ndims(y);

% Some modifications are required if Y is a vector
if isvector(y)
    dimy = 1;
    if size(y,1)==1
        if DIM==1, w = []; return
        elseif DIM==2, DIM=1;
        end
        y = y.';
    elseif DIM==2, w = []; return
    end
end

% Weighing vectors
if ~exist('w','var') || isempty(w)
    w = cell(1,dimy);
    for dim = 1:dimy
        if ~isempty(DIM) && dim~=DIM, continue, end
        n = (dimy==1)*numel(y) + (dimy>1)*sizy(dim);
        w{dim} = exp(1i*(0:n-1)'*pi/2/n);
    end
end

% --- IDCT algorithm ---
if ~isreal(y)
    y = complex(idctn(real(y),DIM,w),idctn(imag(y),DIM,w));
else
    for dim = 1:dimy
        if ~isempty(DIM) && dim~=DIM
            y = shiftdim(y,1);
            continue
        end
        siz = size(y);
        n = siz(1);
        y = reshape(y,n,[]);
        y = bsxfun(@times,y,w{dim});
        y(1,:) = y(1,:)/sqrt(2);
        y = ifft(y,[],1);
        y = real(y*sqrt(2*n));
        I = (1:n)*0.5+0.5;
        I(2:2:end) = n-I(1:2:end-1)+1;
        y = y(I,:);
        y = reshape(y,siz);
        y = shiftdim(y,1);
    end
end

y = reshape(y,sizy);


function w = hann(n)
% function w = hann(n)
% A Hann window of length n. Does not smear tones located exactly in a bin.
w = .5*(1 - cos(2*pi*(0:n-1)/n) );


% 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 [N,binEdges] = BF_SimpleBinner(xData,numBins)
% BF_SimpleBinner Generate a histogram from equally spaced bins.
% An alternative to the more opaque method of Matlab's internal histcounts function.
%
%---INPUTS:
% xData, a data vector.
% numBins, the number of bins.
%---OUTPUTS:
% N, the counts
% binEdges, the extremeties of the bins.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This work is licensed under the Creative Commons
% Attribution-NonCommercial-ShareAlike 4.0 International License. To view a copy of
% this license, visit http://creativecommons.org/licenses/by-nc-sa/4.0/ or send
% a letter to Creative Commons, 444 Castro Street, Suite 900, Mountain View,
% California, 94041, USA.
% ------------------------------------------------------------------------------

minX = min(xData);
maxX = max(xData);

% Linearly spaced bins:
binEdges = linspace(minX,maxX,numBins+1);

N = zeros(numBins,1);
for i = 1:numBins
    if i < numBins
        N(i) = sum(xData >= binEdges(i) & xData < binEdges(i+1));
    else
        % the final bin
        N(i) = sum(xData >= binEdges(i) & xData <= binEdges(i+1));
    end
end

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 [ x, seed ] = uniform_sample ( a, b, seed )

%*****************************************************************************80
%
%% UNIFORM_SAMPLE samples the Uniform PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    A < B.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  [ cdf, seed ] = r8_uniform_01 ( seed );

  x = uniform_cdf_inv ( cdf, a, b );

  return
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.' );