% Minimize sidelobe level of an array with arbitrary 2-D geometry
% "Convex optimization examples" lecture notes (EE364) by S. Boyd
% "Antenna array pattern synthesis via convex optimization"
% by H. Lebret and S. Boyd
% (figures are generated)
%
% Designs an antenna array such that:
% - it minimizes sidelobe level outside the beamwidth of the pattern
% - it has a unit sensitivity at some target direction
% - it has nulls (zero sensitivity) at specified direction(s) (optional)
%
% This is a convex problem (after sampling it can be formulated as an SOCP).
%
%   minimize   max |y(theta)|     for theta outside the beam
%       s.t.   y(theta_tar) = 1
%              y(theta_null) = 0  (optional)
%
% where y is the antenna array gain pattern (complex function) and
% variables are w (antenna array weights or shading coefficients).
% Gain pattern is a linear function of w: y(theta) = w'*a(theta)
% for some a(theta) describing antenna array configuration and specs.
%
% Written for CVX by Almir Mutapcic 02/02/06

% select array geometry
ARRAY_GEOMETRY = '2D_RANDOM';
% ARRAY_GEOMETRY = '1D_UNIFORM_LINE';
% ARRAY_GEOMETRY = '2D_UNIFORM_LATTICE';

% select if the optimal array pattern should enforce nulls or not
HAS_NULLS = 0; % HAS_NULLS = 1;

%********************************************************************
% problem specs
%********************************************************************
lambda = 1;           % wavelength
theta_tar = 60;       % target direction (should be an integer -- discretization)
half_beamwidth = 10;  % half beamwidth around the target direction

% angles where we want nulls (optional)
if HAS_NULLS
    theta_nulls = [95 110 120 140 225];
end

%********************************************************************
% random array of n antenna elements
%********************************************************************
if strcmp( ARRAY_GEOMETRY, '2D_RANDOM' )
  % set random seed to repeat experiments
  rand('state',0);

  % (uniformly distributed on [0,L]-by-[0,L] square)
  n = 40;
  L = 5;
  loc = L*rand(n,2);
  angleRange = 360;

%********************************************************************
% uniform 1D array with n elements with inter-element spacing d
%********************************************************************
elseif strcmp( ARRAY_GEOMETRY, '1D_UNIFORM_LINE' )
  % (unifrom array on a line)
  n = 30;
  d = 0.45*lambda;
  loc = [d*(0:n-1)' zeros(n,1)];
  angleRange = 180;

%********************************************************************
% uniform 2D array with m-by-m element with d spacing
%********************************************************************
elseif strcmp( ARRAY_GEOMETRY, '2D_UNIFORM_LATTICE' )
  m = 6; n = m^2;
  d = 0.45*lambda;

  loc = zeros(n,2);
  for x = 0:m-1
    for y = 0:m-1
      loc(m*y+x+1,:) = [x y];
    end
  end
  loc = loc*d;
  angleRange = 360;

else
  error('Undefined array geometry')
end

%********************************************************************
% construct optimization data
%********************************************************************
% build matrix A that relates w and y(theta), ie, y = A*w
theta = (1:angleRange)';
A = kron(cos(pi*theta/180), loc(:,1)') + kron(sin(pi*theta/180), loc(:,2)');
A = exp(2*pi*1i/lambda*A);

% target constraint matrix
[diff_closest, ind_closest] = min( abs(theta - theta_tar) );
Atar = A(ind_closest,:);

% nulls constraint matrix
if HAS_NULLS
  Anull = []; ind_nulls = [];
  for k = 1:length(theta_nulls)
    [diff_closest, ind_closest] = min( abs(theta - theta_nulls(k)) );
    Anull = [Anull; A(ind_closest,:)]; %#ok
    ind_nulls = [ind_nulls ind_closest]; %#ok
  end
end

% stopband constraint matrix
ind = find(theta <= (theta_tar-half_beamwidth) | ...
           theta >= (theta_tar+half_beamwidth) );
if HAS_NULLS,
    ind = setdiff(ind,ind_nulls);
end;
As = A(ind,:);

%********************************************************************
% optimization problem
%********************************************************************
cvx_begin
  variable w(n) complex
  minimize( max( abs(As*w) ) )
  subject to
    Atar*w == 1;   %#ok target constraint
    if HAS_NULLS   % nulls constraints
      Anull*w == 0; %#ok
    end
cvx_end

% check if problem was successfully solved
disp(['Problem is ' cvx_status])
if ~strfind(cvx_status,'Solved')
  return
end

min_sidelobe_level = 20*log10( max(abs(As*w)) );
fprintf(1,'The minimum sidelobe level is %3.2f dB.\n\n',...
          min_sidelobe_level );

%********************************************************************
% plots
%********************************************************************
figure(1), clf
plot(loc(:,1),loc(:,2),'o')
title('Antenna locations')

% plot array pattern
if angleRange == 180,
    theta = (1:360)';
    A = [ A; -A ];
end
y = A*w;
figure(2), clf
ymin = floor(0.1*min_sidelobe_level)*10-10; ymax = 0;
plot(1:360, 20*log10(abs(y)), ...
     [theta_tar theta_tar],[ymin ymax],'r--',...
     [theta_tar+half_beamwidth theta_tar+half_beamwidth],[ymin ymax],'g--',...
     [theta_tar-half_beamwidth theta_tar-half_beamwidth],[ymin ymax],'g--');
if HAS_NULLS % add lines that represent null positions
  hold on;
  for k = 1:length(theta_nulls)
    plot([theta_nulls(k) theta_nulls(k)],[ymin ymax],'m--');
  end
  hold off;
end
xlabel('look angle'), ylabel('mag y(theta) in dB');
axis([0 360 ymin ymax]);

% polar plot
figure(3), clf
zerodB = -ymin;
dBY = 20*log10(abs(y)) + zerodB;
ind = find( dBY <= 0 ); dBY(ind) = 0;
plot(dBY.*cos(pi*theta/180), dBY.*sin(pi*theta/180), '-');
axis([-zerodB zerodB -zerodB zerodB]), axis('off'), axis('square')
hold on
plot(zerodB*cos(pi*theta/180),zerodB*sin(pi*theta/180),'k:') % 0 dB
plot( (min_sidelobe_level + zerodB)*cos(pi*theta/180), ...
      (min_sidelobe_level + zerodB)*sin(pi*theta/180),'k:')  % min level
text(-zerodB,0,'0 dB')
tt = text(-(min_sidelobe_level + zerodB),0,sprintf('%0.1f dB',min_sidelobe_level));
set(tt,'HorizontalAlignment','right');
theta_1 = theta_tar+half_beamwidth;
theta_2 = theta_tar-half_beamwidth;
plot([0 55*cos(theta_tar*pi/180)], [0 55*sin(theta_tar*pi/180)], 'k:')
plot([0 55*cos(theta_1*pi/180)], [0 55*sin(theta_1*pi/180)], 'k:')
plot([0 55*cos(theta_2*pi/180)], [0 55*sin(theta_2*pi/180)], 'k:')
if HAS_NULLS % add lines that represent null positions
  for k = 1:length(theta_nulls)
    plot([0 55*cos(theta_nulls(k)*pi/180)], ...
         [0 55*sin(theta_nulls(k)*pi/180)], 'k:')
  end
end
hold off


function result=hl(x)

%HL computes the Hodges-Lehmann location estimate on the columns of x.
% The Hodges-Lehmann estimator is defined as
%           hl(x)=med {(x_i + x_j)/2} with 1<=i<j<=n
% It can resist about 29% outliers.
% If x is a matrix, the location estimate is computed on the columns of x. The
% result is then a row vector. If x is a row or a column vector,
% the output is a scalar.
%
% The behavior of the HL estimator in small samples is discussed in:
%   Rousseeuw, P.J. and Verboven, S. (2002),
%   "Robust estimation in very small samples",
%   Computational Statistics and Data Analysis, 40, 741-758.
%
% Required input argument:
%    x: either a data matrix with n observations in rows, p variables in columns
%       or a vector of length n.
%
% I/O:  result=hl(x);
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by S. Verboven
% Last revision: 28/08/03 by N. Smets

[n,p]=size(x);

if n==1 & p==1
    result=x;     %when X is a one by one matrix, all location estimators must be equal to that matrix
    return
elseif n==1
    x=x';      %we only want to work with column vectors
    n=p;
    p=1;
end

if n==2
    result=mean(x,1);  % all location estimators must equal to the average for n=2
    return
end

for k=1:p
    m=0;
    y=zeros(1,n*(n-1)/2); %initializing help vector
    X=x(:,k);
    for i=1:n              %calculating all possible pairwise means
        for j=(i+1):n
            m=m+1;
            y(m)=(X(i)+X(j))/2;
        end
    end
    result(k)=median(y);
    y=[];                 %clear help vector
end


function c = acorr2(x,maxlag)
%ACORR Estimate autocorrelation function of time series
%
%   C = ACORR(X,MAXLAG) returns normalized autocorrelation
%   sequences for each column of X computing correlation via FFT
%

% Copyright (C) 2000-2008 Aki Vehtari
%
% This software is distributed under the GNU General Public
% Licence (version 3 or later); please refer to the file
% Licence.txt, included with the software, for details.

if nargin < 1
  error('Not enough input arguments.');
end
if nargin < 2
  maxlag=length(x)-1;
end
[m,n]=size(x);
c=zeros(maxlag,n);
for i1=1:n
  xn = x(:,i1)-mean(x(:,i1));
  xf = fft(xn,2^nextpow2(2*m-1));
  cf = ifft(abs(xf).^2);
  c(:,i1) = cf(2:(maxlag+1))./cf(1);
end


function [Ixx, Iyy, Ixy] = polygonSecondAreaMoments(poly)
%POLYGONSECONDAREAMOMENTS Compute second-order area moments of a polygon.
%
%   [IXX, IYY, IXY] = polygonSecondAreaMoments(POLY)
%   Compute the second-order inertia moments of a polygon. The polygon is
%   specified by the N-by-2 list of vertex coordinates.
%
%   Example
%   polygonSecondAreaMoments
%
%   References
%   * http://paulbourke.net/geometry/polygonmesh/
%   * https://en.wikipedia.org/wiki/Second_moment_of_area
%
%   See also
%     polygons2d, polygonEquivalentEllipse, polygonArea, polygonCentroid

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2017-09-08, using Matlab 9.1.0.441655 (R2016b)
% Copyright 2017-2022 INRA - Cepia Software Platform

poly = parsePolygon(poly, 'repetition');

px = poly(:,1);
py = poly(:,2);

% vertex indices
N = length(px);
iNext = [2:N 1];

% compute twice signed area of each triangle
common = px .* py(iNext) - px(iNext) .* py;

% compute each term
Ixx = sum( (py.^2 + py .* py(iNext) + py(iNext).^2) .* common) / 12;
Iyy = sum( (px.^2 + px .* px(iNext) + px(iNext).^2) .* common) / 12;
Ixy = sum( ...
    (px .* py(iNext) + 2 * px .* py + 2 * px(iNext) .* py(iNext) ...
    + px(iNext) .* py ) .* common) / 24;


% EX_LIN_ELAST_CUBE_MP: solve linear elasticity problem in a multipatch cube.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_2cubesa.txt';
% You can see how the patches should match in the files
%  geo_2cubesa{b,c,d,e,f,g,h}.txt
% In all the eight cases the result must be the same

% Type of boundary conditions for each side of the domain
problem_data.nmnn_sides   = [];
problem_data.drchlt_sides = [1 2 3 4 5 6];

% Physical parameters
E  =  1; nu = 0.3;
problem_data.lambda_lame = @(x, y, z) ((nu*E)/((1+nu)*(1-2*nu)) * ones (size (x)));
problem_data.mu_lame = @(x, y, z) (E/(2*(1+nu)) * ones (size (x)));

% Source and boundary terms
fx = @(x, y, z) problem_data.mu_lame(x, y, z) .* (2*cos(x) - 4*x.*y.^2.*z) + ...
                problem_data.lambda_lame(x, y, z) .* (cos(x) - 4*x.*y.^2.*z);
fy = @(x, y, z) problem_data.mu_lame(x, y, z) .* (2*sin(y).*z - 4*x.^2.*y.*z) + ...
                problem_data.lambda_lame(x, y, z) .* (z.*sin(y) - 4*x.^2.*y.*z);
fz = @(x, y, z) problem_data.mu_lame(x, y, z) .* (-2*(y.*z).^2 - 2*(x.*z).^2 - cos(y) - 4*(x.*y).^2) - ...
                problem_data.lambda_lame(x, y, z) .* (2*(x.*y).^2 + cos(y));
problem_data.f = @(x, y, z) cat(1, ...
                    reshape (fx (x,y,z), [1, size(x)]), ...
                    reshape (fy (x,y,z), [1, size(x)]), ...
                    reshape (fz (x,y,z), [1, size(x)]));
problem_data.h = @(x, y, z, ind) cat(1, ...
                    reshape (cos(x), [1, size(x)]), ...
                    reshape (sin(y).*z, [1, size(x)]), ...
                    reshape ((x.*y.*z).^2, [1, size(x)]));

% Exact solution (optional)
uxex = @(x, y, z) cos(x);
uyex = @(x, y, z) sin(y).*z;
uzex = @(x, y, z) (x.*y.*z).^2;
problem_data.uex  = @(x, y, z) cat(1, ...
		      reshape (uxex (x,y,z), [1, size(x)]), ...
		      reshape (uyex (x,y,z), [1, size(x)]), ...
		      reshape (uzex (x,y,z), [1, size(x)]));

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.degree     = [2 2 2];     % Degree of the bsplines
method_data.regularity = [1 1 1];     % Regularity of the splines
method_data.nsub       = [2 2 2];     % Number of subdivisions
method_data.nquad      = [3 3 3];     % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER
[geometry, msh, space, u] = mp_solve_linear_elasticity (problem_data, method_data);

% 4) POST-PROCESSING.
% EXPORT TO PARAVIEW
output_file = 'lin_elast_cube_mp_Deg2_Reg1_Sub2';

vtk_pts = {linspace(0, 1, 10), linspace(0, 1, 10), linspace(0, 1, 10)};
fprintf ('results being saved in: %s.pvd\n \n', output_file)
sp_to_vtk (u, space, geometry, vtk_pts, output_file, {'displacement', 'stress'}, {'value', 'stress'}, ...
    problem_data.lambda_lame, problem_data.mu_lame)

% COMPARISON WITH THE EXACT SOLUTION
error_l2 = sp_l2_error (space, msh, u, problem_data.uex)

%!demo
%! ex_lin_elast_cube_mp

%!test
%! problem_data.geo_name = 'geo_2cubesa.txt';
%! problem_data.nmnn_sides   = [];
%! problem_data.drchlt_sides = [1 2 3 4 5 6];
%! E  =  1; nu = 0.3;
%! problem_data.lambda_lame = @(x, y, z) ((nu*E)/((1+nu)*(1-2*nu)) * ones (size (x)));
%! problem_data.mu_lame = @(x, y, z) (E/(2*(1+nu)) * ones (size (x)));
%! fx = @(x, y, z) problem_data.mu_lame(x, y, z) .* (2*cos(x) - 4*x.*y.^2.*z) + ...
%!                 problem_data.lambda_lame(x, y, z) .* (cos(x) - 4*x.*y.^2.*z);
%! fy = @(x, y, z) problem_data.mu_lame(x, y, z) .* (2*sin(y).*z - 4*x.^2.*y.*z) + ...
%!                 problem_data.lambda_lame(x, y, z) .* (z.*sin(y) - 4*x.^2.*y.*z);
%! fz = @(x, y, z) problem_data.mu_lame(x, y, z) .* (-2*(y.*z).^2 - 2*(x.*z).^2 - cos(y) - 4*(x.*y).^2) - ...
%!                 problem_data.lambda_lame(x, y, z) .* (2*(x.*y).^2 + cos(y));
%! problem_data.f = @(x, y, z) cat(1, ...
%!                     reshape (fx (x,y,z), [1, size(x)]), ...
%!                     reshape (fy (x,y,z), [1, size(x)]), ...
%!                     reshape (fz (x,y,z), [1, size(x)]));
%! problem_data.h = @(x, y, z, ind) cat(1, ...
%!                     reshape (cos(x), [1, size(x)]), ...
%!                     reshape (sin(y).*z, [1, size(x)]), ...
%!                     reshape ((x.*y.*z).^2, [1, size(x)]));
%! uxex = @(x, y, z) cos(x);
%! uyex = @(x, y, z) sin(y).*z;
%! uzex = @(x, y, z) (x.*y.*z).^2;
%! problem_data.uex  = @(x, y, z) cat(1, ...
%! 		      reshape (uxex (x,y,z), [1, size(x)]), ...
%! 		      reshape (uyex (x,y,z), [1, size(x)]), ...
%! 		      reshape (uzex (x,y,z), [1, size(x)]));
%! method_data.degree     = [2 2 2];     % Degree of the bsplines
%! method_data.regularity = [1 1 1];     % Regularity of the splines
%! method_data.nsub       = [2 2 2];     % Number of subdivisions
%! method_data.nquad      = [3 3 3];     % Points for the Gaussian quadrature rule
%! [geometry, msh, space, u] = mp_solve_linear_elasticity (problem_data, method_data);
%! error_l2 = sp_l2_error (space, msh, u, problem_data.uex);
%! assert (space.ndof, 504)
%! assert (error_l2, 2.42330537409875e-04, 1e-16)
%!
%! problem_data.geo_name = 'geo_2cubesb.txt';
%! [geometry, msh, space, u] = mp_solve_linear_elasticity (problem_data, method_data);
%! error_l2 = sp_l2_error (space, msh, u, problem_data.uex);
%! assert (space.ndof, 504)
%! assert (error_l2, 2.42330537409875e-04, 1e-16)
%!
%! problem_data.geo_name = 'geo_2cubesc.txt';
%! [geometry, msh, space, u] = mp_solve_linear_elasticity (problem_data, method_data);
%! error_l2 = sp_l2_error (space, msh, u, problem_data.uex);
%! assert (space.ndof, 504)
%! assert (error_l2, 2.42330537409875e-04, 1e-16)
%!
%! problem_data.geo_name = 'geo_2cubesd.txt';
%! [geometry, msh, space, u] = mp_solve_linear_elasticity (problem_data, method_data);
%! error_l2 = sp_l2_error (space, msh, u, problem_data.uex);
%! assert (space.ndof, 504)
%! assert (error_l2, 2.42330537409875e-04, 1e-16)
%!
%! problem_data.geo_name = 'geo_2cubese.txt';
%! [geometry, msh, space, u] = mp_solve_linear_elasticity (problem_data, method_data);
%! error_l2 = sp_l2_error (space, msh, u, problem_data.uex);
%! assert (space.ndof, 504)
%! assert (error_l2, 2.42330537409875e-04, 1e-16)
%!
%! problem_data.geo_name = 'geo_2cubesf.txt';
%! [geometry, msh, space, u] = mp_solve_linear_elasticity (problem_data, method_data);
%! error_l2 = sp_l2_error (space, msh, u, problem_data.uex);
%! assert (space.ndof, 504)
%! assert (error_l2, 2.42330537409875e-04, 1e-16)
%!
%! problem_data.geo_name = 'geo_2cubesg.txt';
%! [geometry, msh, space, u] = mp_solve_linear_elasticity (problem_data, method_data);
%! error_l2 = sp_l2_error (space, msh, u, problem_data.uex);
%! assert (space.ndof, 504)
%! assert (error_l2, 2.42330537409875e-04, 1e-16)
%!
%! problem_data.geo_name = 'geo_2cubesh.txt';
%! [geometry, msh, space, u] = mp_solve_linear_elasticity (problem_data, method_data);
%! error_l2 = sp_l2_error (space, msh, u, problem_data.uex);
%! assert (space.ndof, 504)
%! assert (error_l2, 2.42330537409875e-04, 1e-16)


function n2 = sp_dist2(x, c)
% DIST2	Calculates squared distance between two sets of points.
% Adapted from Netlab neural network software:
% http://www.ncrg.aston.ac.uk/netlab/index.php
%
%	Description
%	D = DIST2(X, C) takes two matrices of vectors and calculates the
%	squared Euclidean distance between them.  Both matrices must be of
%	the same column dimension.  If X has M rows and N columns, and C has
%	L rows and N columns, then the result has M rows and L columns.  The
%	I, Jth entry is the  squared distance from the Ith row of X to the
%	Jth row of C.
%
%
%	Copyright (c) Ian T Nabney (1996-2001)

[ndata, dimx] = size(x);
[ncentres, dimc] = size(c);
if dimx ~= dimc
	error('Data dimension does not match dimension of centres')
end

n2 = (ones(ncentres, 1) * sum((x.^2)', 1))' + ...
  ones(ndata, 1) * sum((c.^2)',1) - ...
  2.*(x*(c'));

% Rounding errors occasionally cause negative entries in n2
if any(any(n2<0))
  n2(n2<0) = 0;
end


%This is a way to do a circular cross correlation of the columns of an
%array.
%
%In this file we take the FFT of the array, the FFT works on the columns
%only, then we make a second array, which is the conjugate of the first, we
%multiply these 2 arrays in an element by element manner, then do the
%inverse FFT.
%After this we circularly shift the columns of the conjugate array, go
%through the process again, then put the results nextto the previous ones.
%
%The first results will be autocorrelation, the subsequent ones will be
%circular cross correlation
%
%This goes pretty quickly if the output array is preallocated, so the method
%was developed to allow preallocation.
%
%The input variables are:
%NRows => The number of Rows of the input array
%NColumns=> guess
%Signal => the matrix with the time data in columns, for instance if you
%       are looking at the cross correlation of orthogonal codes, each code
%       stream would be a column, of course this is NRows by NColumns
%
%You can change this m-file to a function if you feel like it

CircXCorrData=zeros(NRows,NColumns^2);%Preallocate space
h = waitbar(0,'Please wait...');
SignalFFT=fft(Signal);
ConjSignalFFT=conj(SignalFFT);
for index= 1 : NColumns : ( (NColumns-1)*NColumns+1 )
    waitbar(index/( (NColumns-1)*NColumns+1 ))
    CircXCorrData( :,index : index+(NColumns-1) )=ifft(SignalFFT.*ConjSignalFFT);
    ConjSignalFFT=circshift(ConjSignalFFT,[0,1]);
end
close(h)

%Comment the plot out if you want
plot(max(abs(CircXCorrData)))
grid on


function cdf = f_cdf ( x, m, n )

%*****************************************************************************80
%
%% F_CDF evaluates the F central CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Formula 26.5.28
%    Abramowitz and Stegun,
%    Handbook of Mathematical Functions.
%
%  Parameters:
%
%    Input, real X, the argument of the CDF.
%
%    Input, integer M, N, the parameters of the PDF.
%    1 <= M,
%    1 <= N.
%
%    Output, real CDF, the value of the CDF.
%
  if ( x <= 0.0 )

    cdf = 0.0;

  else

    arg1 = 0.5 * n;
    arg2 = 0.5 * m;
    arg3 = n / ( n + m * x );

    cdf = beta_inc ( arg1, arg2, arg3 );

  end

  return
end


function [center, U, obj_fcn] = my_fcm(data, cluster_n, options)
%FCM Data set clustering using fuzzy c-means clustering.
%
%   [CENTER, U, OBJ_FCN] = FCM(DATA, N_CLUSTER) finds N_CLUSTER number of
%   clusters in the data set DATA. DATA is size M-by-N, where M is the number of
%   data points and N is the number of coordinates for each data point. The
%   coordinates for each cluster center are returned in the rows of the matrix
%   CENTER. The membership function matrix U contains the grade of membership of
%   each DATA point in each cluster. The values 0 and 1 indicate no membership
%   and full membership respectively. Grades between 0 and 1 indicate that the
%   data point has partial membership in a cluster. At each iteration, an
%   objective function is minimized to find the best location for the clusters
%   and its values are returned in OBJ_FCN.
%
%   [CENTER, ...] = FCM(DATA,N_CLUSTER,OPTIONS) specifies a vector of options
%   for the clustering process:
%       OPTIONS(1): exponent for the matrix U             (default: 2.0)
%       OPTIONS(2): maximum number of iterations          (default: 100)
%       OPTIONS(3): minimum amount of improvement         (default: 1e-5)
%       OPTIONS(4): info display during iteration         (default: 1)
%   The clustering process stops when the maximum number of iterations
%   is reached, or when the objective function improvement between two
%   consecutive iterations is less than the minimum amount of improvement
%   specified. Use NaN to select the default value.
%
%   Example
%       data = rand(100,2);
%       [center,U,obj_fcn] = fcm(data,2);
%       plot(data(:,1), data(:,2),'o');
%       hold on;
%       maxU = max(U);
%       % Find the data points with highest grade of membership in cluster 1
%       index1 = find(U(1,:) == maxU);
%       % Find the data points with highest grade of membership in cluster 2
%       index2 = find(U(2,:) == maxU);
%       line(data(index1,1),data(index1,2),'marker','*','color','g');
%       line(data(index2,1),data(index2,2),'marker','*','color','r');
%       % Plot the cluster centers
%       plot([center([1 2],1)],[center([1 2],2)],'*','color','k')
%       hold off;
%
%   See also FCMDEMO, INITFCM, IRISFCM, DISTFCM, STEPFCM.

%   Roger Jang, 12-13-94, N. Hickey 04-16-01
%   Copyright 1994-2002 The MathWorks, Inc.
%   $Revision: 1.13 $  $Date: 2002/04/14 22:20:38 $

if nargin ~= 2 & nargin ~= 3,
	error('Too many or too few input arguments!');
end

data_n = size(data, 1);
in_n = size(data, 2);

% Change the following to set default options
default_options = [2;	% exponent for the partition matrix U
		100;	% max. number of iteration
		1e-5;	% min. amount of improvement
		1];	% info display during iteration

if nargin == 2,
	options = default_options;
else
	% If "options" is not fully specified, pad it with default values.
	if length(options) < 4,
		tmp = default_options;
		tmp(1:length(options)) = options;
		options = tmp;
	end
	% If some entries of "options" are nan's, replace them with defaults.
	nan_index = find(isnan(options)==1);
	options(nan_index) = default_options(nan_index);
	if options(1) <= 1,
		error('The exponent should be greater than 1!');
	end
end

expo = options(1);		% Exponent for U
max_iter = options(2);		% Max. iteration
min_impro = options(3);		% Min. improvement
display = options(4);		% Display info or not

obj_fcn = zeros(max_iter, 1);	% Array for objective function

U = my_initfcm(cluster_n, data_n);	% changed by Romano Cicchetti in order to obtain reproducable results
% Main loop
for i = 1:max_iter,
	[U, center, obj_fcn(i)] = stepfcm(data, U, cluster_n, expo);
	if display,
		fprintf('Iteration count = %d, obj. fcn = %f\n', i, obj_fcn(i));
	end
	% check termination condition
	if i > 1,
		if abs(obj_fcn(i) - obj_fcn(i-1)) < min_impro, break; end,
	end
end

iter_n = i;	% Actual number of iterations
obj_fcn(iter_n+1:max_iter) = [];


function spline_test16 ( )

%*****************************************************************************80
%
%% TEST16 tests SPLINE_CUBIC_SET, SPLINE_CUBIC_VAL2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author
%
%    John Burkardt
%
  n = 11;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST16\n' );
  fprintf ( 1, '  SPLINE_CUBIC_SET sets up a cubic spline;\n' );
  fprintf ( 1, '  SPLINE_CUBIC_VAL2 evaluates it.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Runge''s function, evenly spaced knots.\n' );

  for i = 1 : n

    t(i) = ( ( n - i     ) * (-1.0E+00)   ...
           + (     i - 1 ) * (+1.0E+00) ) ...
           / ( n     - 1 );

    y(i) =  frunge ( t(i) );

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The data to be interpolated:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of data values = %d\n', n );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       T             Y\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '%14f  %14f\n', t(i), y(i) );
  end
%
%  Try boundary condition types 0, 1 and 2.
%
  for k = 0 : 2

    if ( k == 0 )

      ibcbeg = 0;
      ybcbeg = 0.0E+00;

      ibcend = 0;
      ybcend = 0.0E+00;

      fprintf ( 1, '\n' );
      fprintf ( 1, '  Boundary condition 0 at both ends:\n' );
      fprintf ( 1, '  Spline is quadratic in boundary intervals.\n' );

    elseif ( k == 1 )

      ibcbeg = 1;
      ybcbeg = fprunge ( t(1) );

      ibcend = 1;
      ybcend = fprunge ( t(n) );

      fprintf ( 1, '\n' );
      fprintf ( 1, '  Boundary condition 1 at both ends:\n' );
      fprintf ( 1, '  Y''(left) =  %f\n', ybcbeg );
      fprintf ( 1, '  Y''(right) = %f\n', ybcend );

    elseif ( k == 2 )

      ibcbeg = 2;
      ybcbeg = fpprunge ( t(1) );

      ibcend = 2;
      ybcend = fpprunge ( t(n) );

      fprintf ( 1, '\n' );
      fprintf ( 1, '  Boundary condition 2 at both ends:\n' );
      fprintf ( 1, '  YP"(left) =  %f\n', ybcbeg );
      fprintf ( 1, '  YP"(right) = %f\n', ybcend );

    end

    ypp = spline_cubic_set ( n, t, y, ibcbeg, ybcbeg, ibcend, ybcend );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  SPLINE"(T), F"(T):\n' );
    fprintf ( 1, '\n' );
    for i = 1 : n
      fprintf ( 1, '%14f  %14f\n', ypp(i), fpprunge(t(i)) );
    end

    left = 0;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  T, SPLINE(T), F(T), LEFT_IN, LEFT_OUT\n' );
    fprintf ( 1, '\n' );

    for i = 0 : n

      if ( i == 0 )
        jhi = 1;
      elseif ( i < n )
        jhi = 2;
      else
        jhi = 2;
      end

      for j = 1 : jhi

        if ( i == 0 )
          tval = t(1) - 1.0E+00;
        elseif ( i < n )
          tval = ( ( jhi - j + 1 ) * t(i)     ...
                 + (       j - 1 ) * t(i+1) ) ...
                 / ( jhi         );
        else
          if ( j == 1 )
            tval = t(n);
          else
            tval = t(n) + 1.0E+00;
          end
        end

        left_in = left;

        [ yval, ypval, yppval ] = ...
          spline_cubic_val2 ( n, t, y, ypp, left, tval );

        fprintf ( 1, '%14f  %14f  %14f  %6d  %6d\n', ...
          tval, yval, frunge ( tval ), left_in, left );

      end
    end

  end

  return
end


function W = randInitializeWeights(L_in, L_out)
%RANDINITIALIZEWEIGHTS Randomly initialize the weights of a layer with L_in
%incoming connections and L_out outgoing connections
%   W = RANDINITIALIZEWEIGHTS(L_in, L_out) randomly initializes the weights
%   of a layer with L_in incoming connections and L_out outgoing
%   connections.
%
%   Note that W should be set to a matrix of size(L_out, 1 + L_in) as
%   the column row of W handles the "bias" terms
%

% You need to return the following variables correctly
W = zeros(L_out, 1 + L_in);

% ====================== YOUR CODE HERE ======================
% Instructions: Initialize W randomly so that we break the symmetry while
%               training the neural network.
%
% Note: The first row of W corresponds to the parameters for the bias units
%

epsilon_init = (6/(L_in))^0.5;

W = rand(L_out,1+L_in)*2*epsilon_init-epsilon_init;

% =========================================================================

end


function ref = physical_to_reference_t3 ( t, n, phy )

%*****************************************************************************80
%
%% PHYSICAL_TO_REFERENCE_T3 maps a physical point to a reference point.
%
%  Discussion:
%
%    Given the vertices of an order 3 physical triangle and a point
%    (X,Y) in the physical triangle, the routine computes the value
%    of the corresponding image point (XSI,ETA) in reference space.
%
%    Note that this routine may also be appropriate for an order 6
%    triangle, if the mapping between reference and physical space
%    is linear.  This implies, in particular, that the sides of the
%    image triangle are straight and that the "midside" nodes in the
%    physical triangle are halfway along the sides of
%    the physical triangle.
%
%  Reference Element T3:
%
%    |
%    1  3
%    |  |\
%    |  | \
%    S  |  \
%    |  |   \
%    |  |    \
%    0  1-----2
%    |
%    +--0--R--1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the X and Y coordinates
%    of the vertices.  The vertices are assumed to be the images of
%    (0,0), (1,0) and (0,1) respectively.
%
%    Input, integer N, the number of points to transform.
%
%    Input, real PHY(2,N), the coordinates of points in the physical space.
%
%    Output, real REF(2,N), the coordinates of the corresponding
%    points in the reference space.
%
  ref(1,1:n) = ( ( t(2,3) - t(2,1) ) * ( phy(1,1:n) - t(1,1) )   ...
               - ( t(1,3) - t(1,1) ) * ( phy(2,1:n) - t(2,1) ) ) ...
             / ( ( t(2,3) - t(2,1) ) * ( t(1,2)   - t(1,1) )   ...
               - ( t(1,3) - t(1,1) ) * ( t(2,2)   - t(2,1) ) );

  ref(2,1:n) = ( ( t(1,2) - t(1,1) ) * ( phy(2,1:n) - t(2,1) )   ...
               - ( t(2,2) - t(2,1) ) * ( phy(1,1:n) - t(1,1) ) ) ...
             / ( ( t(2,3) - t(2,1) ) * ( t(1,2)   - t(1,1) )   ...
               - ( t(1,3) - t(1,1) ) * ( t(2,2)   - t(2,1) ) );

  return
end


function value = hankel_n_condition ( n )

%*****************************************************************************80
%
%% HANKEL_N_CONDITION returns the L1 condition of the HANKEL_N matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real VALUE, the L1 condition.
%
  v = zeros ( n, 1 );

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

  a_norm = ( n * ( n + 1 ) ) / 2;
  b_norm = sum ( abs ( v(1:n) ) );
  value = a_norm * b_norm;

  return
end


function prob_test105 ( )

%*****************************************************************************80
%
%% TEST105 tests LOG_UNIFORM_MEAN, LOG_UNIFORM_SAMPLE, LOG_UNIFORM_VARIANCE;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST105\n' );
  fprintf ( 1, '  For the Log Uniform PDF:\n' );
  fprintf ( 1, '  LOG_UNIFORM_MEAN computes the mean;\n' );
  fprintf ( 1, '  LOG_UNIFORM_SAMPLE samples;\n' );
  fprintf ( 1, '  LOG_UNIFORM_VARIANCE computes the variance;\n' );

  a = 2.0;
  b = 20.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A =             %14f\n', a );
  fprintf ( 1, '  PDF parameter B =             %14f\n', b );

  check = log_uniform_check ( a, b );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST105 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  mean = log_uniform_mean ( a, b );
  variance = log_uniform_variance ( a, b );

  fprintf ( 1, '  PDF mean =                    %14f\n', mean );
  fprintf ( 1, '  PDF variance =                %14f\n', variance );

  for i = 1 : nsample
    [ x(i), seed ] = log_uniform_sample ( a, b, seed );
  end

  mean = r8vec_mean ( nsample, x );
  variance = r8vec_variance ( nsample, x );
  xmax = max ( x(1:nsample) );
  xmin = min ( x(1:nsample) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =     %6d\n', nsample );
  fprintf ( 1, '  Sample mean =     %14f\n', mean );
  fprintf ( 1, '  Sample variance = %14f\n', variance );
  fprintf ( 1, '  Sample maximum =  %14f\n', xmax );
  fprintf ( 1, '  Sample minimum =  %14f\n', xmin );

  return
end


function [ n_data_new, x, y, fxy ] = beta_values ( n_data )

%*****************************************************************************80
%
%% BETA_VALUES returns some values of the Beta function.
%
%  Formula:
%
%    BETA(X,Y) = ( GAMMA(X) * GAMMA(Y) ) / GAMMA(X+Y)
%
%  Restrictions:
%
%    Both X and Y must be greater than 0.
%
%  Properties:
%
%    BETA(X,Y) = BETA(Y,X).
%    BETA(X,Y) = Integral ( 0 <= T <= 1 ) T**(X-1) (1-T)**(Y-1) dT.
%    BETA(X,Y) = GAMMA(X) * GAMMA(Y) / GAMMA(X+Y)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%  Parameters:
%
%    Input, integer N_DATA, indicates the index of the previous test data
%    returned, or is 0 if this is the first call.  For repeated calls,
%    set the input value of N_DATA to the output value of N_DATA_NEW
%    from the previous call.
%
%    Output, integer N_DATA_NEW, the index of the test data.
%
%    Output, real X, Y, the arguments of the function.
%
%    Output, real FXY, the value of the function.
%
  n_max = 17;
  b_vec = [ ...
    5.000000E+00, 2.500000E+00, 1.666667E+00, 1.250000E+00, ...
    5.000000E+00, 2.500000E+00, 1.000000E+00, 1.666667E-01, ...
    0.333333E-01, 7.142857E-03, 1.587302E-03, 0.238095E-01, ...
    5.952381E-03, 1.984127E-03, 7.936508E-04, 3.607504E-04, ...
    8.325008E-05 ];
  x_vec = [ ...
    0.2E+00, 0.4E+00, 0.6E+00, 0.8E+00, ...
    1.0E+00, 1.0E+00, 1.0E+00, 2.0E+00, ...
    3.0E+00, 4.0E+00, 5.0E+00, 6.0E+00, ...
    6.0E+00, 6.0E+00, 6.0E+00, 6.0E+00, ...
    7.0E+00 ];
  y_vec = [ ...
    1.0E+00, 1.0E+00, 1.0E+00, 1.0E+00, ...
    0.2E+00, 0.4E+00, 1.0E+00, 2.0E+00, ...
    3.0E+00, 4.0E+00, 5.0E+00, 2.0E+00, ...
    3.0E+00, 4.0E+00, 5.0E+00, 6.0E+00, ...
    7.0E+00 ];

  n_data_new = n_data;

  if ( n_data_new < 0 )
    n_data_new = 0;
  end

  n_data_new = n_data_new + 1;

  if ( n_max < n_data_new )
    n_data_new = 0;
    x = 0.0E+00;
    y = 0.0E+00;
    fxy = 0.0E+00;
  else
    x = x_vec(n_data_new);
    y = y_vec(n_data_new);
    fxy = b_vec(n_data_new);
  end

  return
end


%% RATE OF CONVERGENCE OF ADAPTIVE FINITE ELEMENT METHOD USING WG ELEMENT
%
% This example is to show the rate of convergence of the lowest order weak
% Galerkin element approximation of the second order elliptic equation.
%
% Reference
%
% L. Chen, J. Wang and X. Ye. A posteriori error estimates for Weak
% Galerkin finite element methods for second order elliptic problems.
% Journal of Scientific Computing, 59(2), 496-511, 2014.

%% Lshape problem
% mesh
[node,elem] = cubemesh([-1,1,-1,1,-1,1],1);
[node,elem] = delmesh(node,elem,'x>0 & y<0');
bdFlag = setboundary3(node,elem,'Dirichlet');
mesh = struct('node',node,'elem',elem,'bdFlag',bdFlag);
% pde
pde = Lshapedata3;
% option
format shorte
option.L0 = 1;
option.maxIt = 50;
option.printlevel = 1;
option.elemType = 'WG';
option.plotflag = 1;
option.maxN = 2e4;
% AFEM
err = afemPoisson3(mesh,pde,option);
% plot
figure;
showrate2(err.N,err.H1,10,'k-*','|| Du-Du_h||',err.N,err.eta,10,'k-+','eta');
% latexerrtable(err.N,[err.H1 err.eta])

%% Jump coefficients
% The diffusion coefficent is piecewise constant with large jump.
[node,elem] = cubemesh([-1,1,-1,1,-1,1],1);
bdFlag = setboundary3(node,elem,'Dirichlet','(x==1) | (x==-1)');
mesh = struct('node',node,'elem',elem,'bdFlag',bdFlag);
% pde
pde = jumpmgdata1;
global epsilon
epsilon = 1e-4;
% option
option.L0 = 3;
option.maxIt = 50;
option.printlevel = 1;
option.elemType = 'WG';
option.plotflag = 1;
option.maxN = 1e4;
option.viewcut = '~(x<=0 & y>=0 & z>=0)';
option.viewangle = [59,20];
% AFEM
err = afemPoisson3(mesh,pde,option);
% plot
figure;
showrate2(err.N,err.H1,10,'k-*','||Du-Du_h||',err.N,err.eta,10,'k-+','eta');
% Note no exact solution to compute the error. Code the energy later on.


function trans = localToGlobal3d(varargin)
%LOCALTOGLOBAL3D Transformation matrix from local to global coordinate system.
%
%   TRANS = localToGlobal3d(CENTER, THETA, PHI, PSI)
%   Compute the transformation matrix from a local (or modelling)
%   coordinate system to the global (or world) coordinate system.
%   This is a low-level function, used by several drawing functions.
%
%   The transform is defined by:
%   - CENTER: the position of the local origin into the world coordinate
%       system
%   - THETA: colatitude, defined as the angle with the Oz axis (between 0
%       and 180 degrees), positive in the direction of the of Oy axis.
%   - PHI: azimut, defined as the angle of the normal with the Ox axis,
%       between 0 and 360 degrees
%   - PSI: intrinsic rotation, corresponding to the rotation of the object
%       around the direction vector, between 0 and 360 degrees
%
%   The resulting transform is obtained by applying (in that order):
%   - Rotation by PSI   around the Z-axis
%   - Rotation by THETA around the Y-axis
%   - Rotation by PHI   around the Z-axis
%   - Translation by vector CENTER
%   This corresponds to Euler ZYZ rotation, using angles PHI, THETA and
%   PSI.
%
%   The 'eulerAnglesToRotation3d' function may better suit your needs as
%   it is more 'natural'.
%
%   Example
%   localToGlobal3d
%
%   See also
%   transforms3d, eulerAnglesToRotation3d

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2009-06-19, using Matlab 7.7.0.471 (R2008b)
% Copyright 2009-2022 INRA - Cepia Software Platform

% extract the components of the transform
if nargin == 1
    % all components are bundled in  the first argument
    var     = varargin{1};
    center  = var(1:3);
    theta   = var(4);
    phi     = var(5);
    psi     = 0;
    if length(var) > 5
        psi = var(6);
    end

elseif nargin == 4
    % arguments = center, then the 3 angles
    center  = varargin{1};
    theta   = varargin{2};
    phi     = varargin{3};
    psi     = varargin{4};

elseif nargin > 4
    % center is given in 3 arguments, then 3 angles
    center  = [varargin{1} varargin{2} varargin{3}];
    theta   = varargin{4};
    phi     = varargin{5};
    psi     = 0;
    if nargin > 5
        psi = varargin{6};
    end
end

% conversion from degrees to radians
k = pi / 180;

% rotation around normal vector axis
rot1    = createRotationOz(psi * k);

% colatitude
rot2    = createRotationOy(theta * k);

% longitude
rot3    = createRotationOz(phi * k);

% shift center
tr      = createTranslation3d(center);

% create final transform by concatenating transforms
trans   = tr * rot3 * rot2 * rot1;


%% NLS1 Original
clc
%Function
fun = @(x,xdata) x(1)*exp(x(2)*xdata);
grad = @(x,xdata) [ exp(x(2).*xdata), x(1).*xdata.*exp(x(2).*xdata)];
%Fitting Data
xdata = [0.9 1.5 13.8 19.8 24.1 28.2 35.2 60.3 74.6 81.3];
ydata = [455.2 428.6 124.1 67.3 43.2 28.1 13.1 -0.4 -1.3 -1.5];
%Setup Options
opts = optiset('solver','matlab','display','iter');
%Build & Solve
Opt = opti('fun',fun,'grad',grad,'data',xdata,ydata,'ndec',2,'options',opts)
x0 = [100; -1]; % Starting guess
[x,fval,exitflag,info] = solve(Opt,x0)
%Plot
confidence(Opt)
plot(Opt)

%% NLS1 Weighted [no grad]
clc
%Function
fun = @(x,xdata) x(1)*exp(x(2)*xdata);
grad = @(x,xdata) [ exp(x(2).*xdata), x(1).*xdata.*exp(x(2).*xdata)];
%Fitting Data
xdata = [0.9 1.5 13.8 19.8 24.1 28.2 35.2 60.3 74.6 81.3];
ydata = [455.2 428.6 124.1 67.3 43.2 28.1 13.1 -0.4 -1.3 -1.5];
%Fitting Weights
weights = ones(size(ydata)); weights(end) = 1e3;
%Setup Options
opts = optiset('solver','lmder','display','iter');
%Build & Solve
Opt = opti('fun',fun,'grad',[],'data',xdata,ydata,'weights',weights,'ndec',2,'options',opts)
x0 = [100; -1]; % Starting guess
[x,fval,exitflag,info] = solve(Opt,x0)
%Plot
[cInt,stats] = confidence(Opt)
plot(Opt)

%% NLS1 Weighted no xdata [no grad]
clc
%Fitting Data
xdata = [0.9 1.5 13.8 19.8 24.1 28.2 35.2 60.3 74.6 81.3]';
ydata = [455.2 428.6 124.1 67.3 43.2 28.1 13.1 -0.4 -1.3 -1.5];
%Function
fun = @(x) x(1)*exp(x(2)*xdata);
grad = @(x) [ exp(x(2).*xdata), x(1).*xdata.*exp(x(2).*xdata)];
%Fitting Weights
weights = ones(size(ydata)); weights(end) = 1e3;
%Setup Options
opts = optiset('solver','lmder','display','iter');
%Build & Solve
Opt = opti('fun',fun,'grad',[],'ydata',ydata,'weights',weights,'ndec',2,'options',opts)
x0 = [100; -1]; % Starting guess
[x,fval,exitflag,info] = solve(Opt,x0)
%Plot
plot(Opt)

%% NLS1 Weighted [with grad]
clc
%Function
fun = @(x,xdata) x(1)*exp(x(2)*xdata);
grad = @(x,xdata) [ exp(x(2).*xdata), x(1).*xdata.*exp(x(2).*xdata)];
%Fitting Data
xdata = [0.9 1.5 13.8 19.8 24.1 28.2 35.2 60.3 74.6 81.3];
ydata = [455.2 428.6 124.1 67.3 43.2 28.1 13.1 -0.4 -1.3 -1.5];
%Fitting Weights
weights = ones(size(ydata)); weights(end) = 1e3;
%Setup Options
opts = optiset('solver','lmder','display','iter');
%Build & Solve
Opt = opti('fun',fun,'grad',grad,'data',xdata,ydata,'weights',weights,'ndec',2,'options',opts)
x0 = [100; -1]; % Starting guess
[x,fval,exitflag,info] = solve(Opt,x0)
%Plot
plot(Opt)

%% NLS1 Weighted no xdata [with grad]
clc
%Fitting Data
xdata = [0.9 1.5 13.8 19.8 24.1 28.2 35.2 60.3 74.6 81.3]';
ydata = [455.2 428.6 124.1 67.3 43.2 28.1 13.1 -0.4 -1.3 -1.5];
%Function
fun = @(x) x(1)*exp(x(2)*xdata);
grad = @(x) [ exp(x(2).*xdata), x(1).*xdata.*exp(x(2).*xdata)];
%Fitting Weights
weights = ones(size(ydata)); weights(end) = 1e3;
%Setup Options
opts = optiset('solver','lmder','display','iter');
%Build & Solve
Opt = opti('fun',fun,'grad',grad,'ydata',ydata,'weights',weights,'ndec',2,'options',opts)
x0 = [100; -1]; % Starting guess
[x,fval,exitflag,info] = solve(Opt,x0)
%Plot
plot(Opt)

%% NLS1 Weighted as NLP [no grad], MANUAL WEIGHTS
clc
%Fitting Data
xdata = [0.9 1.5 13.8 19.8 24.1 28.2 35.2 60.3 74.6 81.3]';
%Fitting Weights
weights = ones(size(xdata)); weights(end) = 1e3;
ydata = [455.2 428.6 124.1 67.3 43.2 28.1 13.1 -0.4 -1.3 -1.5]'.*weights;
%Function
fun = @(x,xdata) x(1)*exp(x(2)*xdata).*weights;
grad = @(x,xdata) [ exp(x(2).*xdata), x(1).*xdata.*exp(x(2).*xdata)];
%Setup Options
opts = optiset('solver','ipopt','display','iter');
%Build & Solve
Opt = opti('fun',fun,'grad',[],'data',xdata,ydata,'weights',[],'ndec',2,'options',opts)
x0 = [100; -1]; % Starting guess
[x,fval,exitflag,info] = solve(Opt,x0)
%Plot
plot(Opt)

%% NLS1 Weighted as NLP [no grad], AUTO WEIGHTS
clc
%Fitting Data
xdata = [0.9 1.5 13.8 19.8 24.1 28.2 35.2 60.3 74.6 81.3]';
ydata = [455.2 428.6 124.1 67.3 43.2 28.1 13.1 -0.4 -1.3 -1.5]';
%Fitting Weights
weights = ones(size(xdata)); weights(end) = 1e3;
%Function
fun = @(x,xdata) x(1)*exp(x(2)*xdata);
grad = @(x,xdata) [ exp(x(2).*xdata), x(1).*xdata.*exp(x(2).*xdata)];
%Setup Options
opts = optiset('solver','ipopt','display','iter');
%Build & Solve
Opt = opti('fun',fun,'grad',[],'data',xdata,ydata,'weights',weights,'ndec',2,'options',opts)
x0 = [100; -1]; % Starting guess
[x,fval,exitflag,info] = solve(Opt,x0)
%Plot
plot(Opt)

%% NLS1 Weighted as NLP no xdata [no grad]
clc
%Fitting Data
xdata = [0.9 1.5 13.8 19.8 24.1 28.2 35.2 60.3 74.6 81.3]';
ydata = [455.2 428.6 124.1 67.3 43.2 28.1 13.1 -0.4 -1.3 -1.5];
%Function
fun = @(x) x(1)*exp(x(2)*xdata);
grad = @(x) [ exp(x(2).*xdata), x(1).*xdata.*exp(x(2).*xdata)];
%Fitting Weights
weights = ones(size(ydata)); weights(end) = 1e3;
%Setup Options
opts = optiset('solver','ipopt','display','iter');
%Build & Solve
Opt = opti('fun',fun,'grad',[],'ydata',ydata,'weights',weights,'ndec',2,'options',opts)
x0 = [100; -1]; % Starting guess
[x,fval,exitflag,info] = solve(Opt,x0)
%Plot
plot(Opt)

%% NLS1 Weighted as NLP [WITH grad], MANUAL WEIGHTS
clc
%Fitting Data
xdata = [0.9 1.5 13.8 19.8 24.1 28.2 35.2 60.3 74.6 81.3]';
%Fitting Weights
weights = ones(size(xdata)); weights(end) = 1e3;
ydata = [455.2 428.6 124.1 67.3 43.2 28.1 13.1 -0.4 -1.3 -1.5]'.*weights;
%Function
fun = @(x,xdata) x(1)*exp(x(2)*xdata).*weights;
grad = @(x,xdata) [ exp(x(2).*xdata).*weights, x(1).*xdata.*exp(x(2).*xdata).*weights];
%Setup Options
opts = optiset('solver','ipopt','display','iter');
%Build & Solve
Opt = opti('fun',fun,'grad',grad,'data',xdata,ydata,'weights',[],'ndec',2,'options',opts)
x0 = [100; -1]; % Starting guess
[x,fval,exitflag,info] = solve(Opt,x0)
%Plot
plot(Opt)

%% NLS1 Weighted as NLP [WITH grad], AUTO WEIGHTS
clc
%Fitting Data
xdata = [0.9 1.5 13.8 19.8 24.1 28.2 35.2 60.3 74.6 81.3]';
ydata = [455.2 428.6 124.1 67.3 43.2 28.1 13.1 -0.4 -1.3 -1.5]';
%Fitting Weights
weights = ones(size(xdata)); weights(end) = 1e3;
%Function
fun = @(x,xdata) x(1)*exp(x(2)*xdata);
grad = @(x,xdata) [ exp(x(2).*xdata), x(1).*xdata.*exp(x(2).*xdata)];
%Setup Options
opts = optiset('solver','ipopt','display','iter');
%Build & Solve
Opt = opti('fun',fun,'grad',grad,'data',xdata,ydata,'weights',weights,'ndec',2,'options',opts)
x0 = [100; -1]; % Starting guess
[x,fval,exitflag,info] = solve(Opt,x0)
%Plot
plot(Opt)

%% NLS1 Weighted as NLP no xdata [WITH grad]
clc
%Fitting Data
xdata = [0.9 1.5 13.8 19.8 24.1 28.2 35.2 60.3 74.6 81.3]';
ydata = [455.2 428.6 124.1 67.3 43.2 28.1 13.1 -0.4 -1.3 -1.5];
%Function
fun = @(x) x(1)*exp(x(2)*xdata);
grad = @(x) [ exp(x(2).*xdata), x(1).*xdata.*exp(x(2).*xdata)];
%Fitting Weights
weights = ones(size(ydata)); weights(end) = 1e3;
%Setup Options
opts = optiset('solver','ipopt','display','iter');
%Build & Solve
Opt = opti('fun',fun,'grad',grad,'ydata',ydata,'weights',weights,'ndec',2,'options',opts)
x0 = [100; -1]; % Starting guess
[x,fval,exitflag,info] = solve(Opt,x0)
%Plot
plot(Opt)

%% NLS2
clc
%Function
i = (1:40)';
fun = @(x) x(1)*exp(-x(2)*i) + x(3);
%Fitting Data
ydata=[5.8728, 5.4948, 5.0081, 4.5929, 4.3574, 4.1198, 3.6843, 3.3642, 2.9742, 3.0237, 2.7002, 2.8781,...
       2.5144, 2.4432, 2.2894, 2.0938, 1.9265, 2.1271, 1.8387, 1.7791, 1.6686, 1.6232, 1.571, 1.6057,...
       1.3825, 1.5087, 1.3624, 1.4206, 1.2097, 1.3129, 1.131, 1.306, 1.2008, 1.3469, 1.1837, 1.2102,...
       0.96518, 1.2129, 1.2003, 1.0743];
%Setup Options
opts = optiset('solver','mkltrnls','display','iter');
%Build & Solve
x0=[1.0; 0.0; 0.0];
Opt = opti('fun',fun,'ydata',ydata,'ndec',3,'options',opts)
[x,fval,exitflag,info] = solve(Opt,x0)

plot(Opt)

%% NLS2 w Weights
clc
%Function
i = (1:40)';
fun = @(x) x(1)*exp(-x(2)*i) + x(3);
%Fitting Data
ydata=[5.8728, 5.4948, 5.0081, 4.5929, 4.3574, 4.1198, 3.6843, 3.3642, 2.9742, 3.0237, 2.7002, 2.8781,...
       2.5144, 2.4432, 2.2894, 2.0938, 1.9265, 2.1271, 1.8387, 1.7791, 1.6686, 1.6232, 1.571, 1.6057,...
       1.3825, 1.5087, 1.3624, 1.4206, 1.2097, 1.3129, 1.131, 1.306, 1.2008, 1.3469, 1.1837, 1.2102,...
       0.96518, 1.2129, 1.2003, 1.0743];
%Weighting
wts = ones(size(ydata)); wts([1 end]) = 1e3;
%Build & Solve
x0=[1.0; 0.0; 0.0];
Opt = opti('fun',fun,'ydata',ydata,'x0',x0);
OptW = opti('fun',fun,'ydata',ydata,'weights',wts,'x0',x0);
x = solve(Opt)
xw = solve(OptW)

plot(i,ydata,'ko',i(end),ydata(end),'ksq',i,fun(xw),'r*-',i,fun(x),'m*-')
xlim([length(i)*0.45 length(i)*1.01]); xlabel('i'); ylabel('y');
legend('Original Data','Target Point','Weighted Fit','Standard Fit');
title('NLS Curve Fit - Comparison of Weighted and Un-weighted');

%% NLS3 (Modified NLP - HS76)
clc
%Function
fun = @(x) [x(1);
            sqrt(0.5)*x(2);
            x(3);
            sqrt(0.5)*x(4);];
%Fitting Data
ydata = [0.0, 0.0, 0.0, 0.0];
%Constraints
lb = [0.0, 0.0, 0.0, 0.0];
ub = [inf, inf, inf, inf];
A = -[-1.0, -2.0, -1.0, -1.0;
     -3.0, -1.0, -2.0, 1.0];
b = -[-5.0, -0.4]';
Aeq = [0.0, 1.0, 4.0, 0.0];
beq = 1.5;
%Setup Options
opts = optiset('solver','levmar','display','iter');
%Build & Solve
x0 = [0.5, 0.5, 0.5, 0.5];
Opt = opti('fun',fun,'ydata',ydata,'ineq',A,b,'eq',Aeq,beq,'bounds',lb,ub,'options',opts)
[x,fval,exitflag,info] = solve(Opt,x0)

%% NLS Example Prob
clc
%Get Problem
prob = nls_prob(19);
opts = optiset('display','iter');
%Build OPTI Object
Opt = opti(prob,opts);
%Solve
[x,fval,exitflag,info] = solve(Opt)
%Plot
plot(Opt,[],1)


function [lat2,lon2,h2]=direct(lat1,lon1,h1,az,va,d,a,e2)
% DIRECT  Computes direct (forward) geodetic problem.
%   Determines coordinates of 2nd station given ellipsoidal
%   coordinates of 1st station and azimuth, vertical angle
%   and distance from 1st to 2nd station. If az,va are local
%   astronomic, lat,lon must also be astronomic. If az,va
%   are local geodetic, lat,lon must be local geodetic.
%   Non-vectorized. See also INVERSE.
% Version: 2011-02-19
% Useage:  [lat2,lon2,h2]=direct(lat1,lon1,h1,az,va,d,a,e2)
%          [lat2,lon2,h2]=direct(lat1,lon1,h1,az,va,d)
% Input:   lat1 - ellipsoidal latitude of 1st station (rads)
%          lon1 - ellipsoidal longitude of 1st station (rads)
%          h1   - ellipsoidal ht. of 1st station (m)
%          az   - azimuth from station 1 to 2 (rads)
%          va   - vertical angle from 1 to 2 (rads)
%          d    - distance from 1 to 2 (m)
%          a    - ref. ellipsoid major semi-axis (m); default GRS80
%          e2   - ref. ellipsoid eccentricity squared; default GRS80
% Output:  lat2 - ellipsoidal latitude of 2nd station (rads)
%          lon2 - ellipsoidal longitude of 2nd station (rads)
%          h2   - ellipsoidal ht. of 2nd station (m)

% Copyright (c) 2011, Michael R. Craymer
% All rights reserved.
% Email: mike@craymer.com

if nargin ~= 6 & nargin ~= 8
  warning('Incorrect number of input arguments');
  return
end
if nargin == 6
  [a,b,e2]=refell('grs80');
end

[X1,Y1,Z1]=ell2xyz(lat1,lon1,h1,a,e2);
[dx,dy,dz]=sph2xyz(az,va,d);
[dX,dY,dZ]=lg2ct(dx,dy,dz,lat1,lon1);
X2=X1+dX;
Y2=Y1+dY;
Z2=Z1+dZ;
[lat2,lon2,h2]=xyz2ell(X2,Y2,Z2,a,e2);


function l = legendre_symbol ( q, p )

%*****************************************************************************80
%
%% LEGENDRE_SYMBOL evaluates the Legendre symbol (Q/P).
%
%  Discussion:
%
%    Let P be an odd prime.  Q is a QUADRATIC RESIDUE modulo P
%    if there is an integer R such that R**2 = Q ( mod P ).
%    The Legendre symbol ( Q / P ) is defined to be:
%
%      + 1 if Q ( mod P ) /= 0 and Q is a quadratic residue modulo P,
%      - 1 if Q ( mod P ) /= 0 and Q is not a quadratic residue modulo P,
%        0 if Q ( mod P ) == 0.
%
%    We can also define ( Q / P ) for P = 2 by:
%
%      + 1 if Q ( mod P ) /= 0
%        0 if Q ( mod P ) == 0
%
%  Example:
%
%    (0/7) =   0
%    (1/7) = + 1  ( 1^2 = 1 mod 7 )
%    (2/7) = + 1  ( 3^2 = 2 mod 7 )
%    (3/7) = - 1
%    (4/7) = + 1  ( 2^2 = 4 mod 7 )
%    (5/7) = - 1
%    (6/7) = - 1
%
%  Note:
%
%    For any prime P, exactly half of the integers from 1 to P-1
%    are quadratic residues.
%
%    ( 0 / P ) = 0.
%
%    ( Q / P ) = ( mod ( Q, P ) / P ).
%
%    ( Q / P ) = ( Q1 / P ) * ( Q2 / P ) if Q = Q1 * Q2.
%
%    If Q is prime, and P is prime and greater than 2, then:
%
%      if ( Q == 1 ) then
%
%        ( Q / P ) = 1
%
%      else if ( Q == 2 ) then
%
%        ( Q / P ) = + 1 if mod ( P, 8 ) = 1 or mod ( P, 8 ) = 7,
%        ( Q / P ) = - 1 if mod ( P, 8 ) = 3 or mod ( P, 8 ) = 5.
%
%      else
%
%        ( Q / P ) = - ( P / Q ) if Q = 3 ( mod 4 ) and P = 3 ( mod 4 ),
%                  =   ( P / Q ) otherwise.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Charles Pinter,
%    A Book of Abstract Algebra,
%    McGraw Hill, 1982, pages 236-237.
%
%    Daniel Zwillinger,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996, pages 86-87.
%
%  Parameters:
%
%    Input, integer Q, an integer whose Legendre symbol with
%    respect to P is desired.
%
%    Input, integer P, a prime number, greater than 1, with respect
%    to which the Legendre symbol of Q is desired.
%
%    Output, integer L, the Legendre symbol (Q/P).
%    Ordinarily, L will be -1, 0 or 1.
%    L = -2, P is less than or equal to 1.
%    L = -3, P is not prime.
%    L = -4, the internal stack of factors overflowed.
%    L = -5, not enough factorization space.
%

%
%  P must be greater than 1.
%
  if ( p <= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LEGENDRE_SYMBOL - Fatal error!\n' );
    fprintf ( 1, '  P must be greater than 1.\n' );
    l = -2;
    error ( 'LEGENDRE_SYMBOL - Fatal error!' );
  end
%
%  P must be prime.
%
  if ( ~i4_is_prime ( p ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LEGENDRE_SYMBOL - Fatal error!\n' );
    fprintf ( 1, '  P is not prime.\n' );
    l = -3;
    error ( 'LEGENDRE_SYMBOL - Fatal error!' );
  end
%
%  ( k*P / P ) = 0.
%
  if ( mod ( q, p ) == 0 )
    l = 0;
    return
  end
%
%  For the special case P = 2, (Q/P) = 1 for all odd numbers.
%
  if ( p == 2 )
    l = 1;
    return
  end
%
%  Make a copy of Q, and force it to be nonnegative.
%
  qq = q;

  while ( qq < 0 )
    qq = qq + p;
  end

  nstack = 0;
  pp = p;
  l = 1;

  while ( 1 )

    qq = mod ( qq, pp );
%
%  Decompose QQ into factors of prime powers.
%
    [ nfactor, factor, power, nleft ] = i4_factor ( qq );

    if ( nleft ~= 1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'LEGENDRE_SYMBOL - Fatal error!\n' );
      fprintf ( 1, '  Not enough factorization space.\n' );
      l = -5;
      error ( 'LEGENDRE_SYMBOL - Fatal error!' );
    end
%
%  Each factor which is an odd power is added to the stack.
%
    nmore = 0;

    for i = 1 : nfactor

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

        nmore = nmore + 1;
        nstack = nstack + 1;

        pstack(nstack) = pp;
        qstack(nstack) = factor(i);

      end

    end

    if ( nmore ~= 0 )

      qq = qstack(nstack);
      nstack = nstack - 1;
%
%  Check for a QQ of 1 or 2.
%
      if ( qq == 1 )

        l = + 1 * l;

      elseif ( qq == 2 & ( mod ( pp, 8 ) == 1 | mod ( pp, 8 ) == 7 ) )

        l = + 1 * l;

      elseif ( qq == 2 & ( mod ( pp, 8 ) == 3 | mod ( pp, 8 ) == 5 ) )

        l = - 1 * l;

      else

        if ( mod ( pp, 4 ) == 3 & mod ( qq, 4 ) == 3 )
          l = - 1 * l;
        end

        [ pp, qq ] = i4_swap ( pp, qq );

        continue

      end

    end
%
%  If the stack is empty, we're done.
%
    if ( nstack == 0 )
      break
    end
%
%  Otherwise, get the last P and Q from the stack, and process them.
%
    pp = pstack(nstack);
    qq = qstack(nstack);
    nstack = nstack - 1;

  end

  return
end


%% CHUTE  Chutes and ladders analysis.
%
%  Discussion:
%
%    This script determines the probability of finishing a one-player
%    game of chutes and ladders in exactly N moves (PDF) or in no more
%    than N moves (CDF).

%
%    The game starts at square 0, and ends when square 100 is reached.
%
%  Modified:
%
%    19 September 2014
%
%  Author:

%

%    Desmond Higham, Nicholas Higham

%

%  Reference:

%

%    Desmond Higham, Nicholas Higham,

%    MATLAB Guide,

%    SIAM, 2005,

%    ISBN13: 9780898717891.

%
  N = 100;
%
% "+1" translates square to state.
%
  top = [ 1  4  9 16 21 28 36 47 49 51 56 62 64 71 80  87 93 95 98] + 1;
  bot = [38 14 31  6 42 84 44 26 11 67 53 19 60 91 100 24 73 75 78] + 1;

  P = toeplitz(zeros(1,N+1),[0 ones(1,6) zeros(1,N-6)]);

  for k = N-4:N+1
    P(k,k) = k-N+5;
  end
  P = P/6;

  for k = 1:length(top)
    r = top(k); s = bot(k);     % Chute or ladder from r to s.
    P(:,s) = P(:,s) + P(:,r);   % Add column r to column s.
  end
  P(top,:) = [];
  P(:,top) = [];  % Remove starts of chutes and ladders.

  figure(1)
  spy(P)

  M = 200;
  cumprob = zeros(M,1);
  cumprob(1) = P(1,end);
  v = P(1,:);
  for n = 2:M,
    v = v*P;
    cumprob(n) = v(end);
  end

  figure(2)
  colormap([0.8,0.4,0.4])
  bar(diff([0;cumprob]))
  title('Probability for Game Length','FontSize',12,'FontWeight','Bold')
  grid on
  xlim([0 M])

figure(3)

colormap([0.8,0.4,0.4])
  bar(cumprob)
  title('Cumulative Probability for Game Length',...
      'FontSize',12,'FontWeight','Bold')
  grid on
  xlim([0 M])


function circle_test ( )

%*****************************************************************************80
%
%% CIRCLE_TEST applies continuation to the circle problem.
%
%  Discussion:
%
%    Our function is
%
%      f(x,y) = x^2 + y^2 - 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 April 2014
%
%  Author:
%
%    John Burkardt
%
  step_max = 35;
  step_num = 0;
  xy = zeros ( 2, step_max );
%
%  A:
%  Choose a starting point,
%  Choose a starting "parameter",
%  Call Newton to try to get starting point to satisfy implicit function
%  while holding parameter fixed.
%
  n = 2;
  x0 = [ 0.5; -2.0 ];
  p0 = 1;
  tol = 1.0E-05;

  fx_norm = max ( abs ( f_circle ( n, x0 ) ) );
  fprintf ( 1, '  %2d  %14.6g  %14.6g  %8.2e\n', 0, x0(1), x0(2), fx_norm );

  [ status, x2 ] = newton ( n, x0, p0, @f_circle, @fp_circle, tol );

  step_num = step_num + 1;

  xy(1:2,step_num) = x2(1:2,1);
  fx_norm = max ( abs ( f_circle ( n, x2 ) ) );
  fprintf ( 1, '  %2d  %14.6g  %14.6g  %8.2e\n', step_num, x2(1), x2(2), fx_norm );
%
%  B:
%  X0 <= X2.
%  Take a step from the current point X0.
%
  x0 = x2;
  t0 = zeros ( n, 1 );
  h = 0.15;

  for step_num = 2 : step_max

    [ status, x2, t2, p2 ] = step ( n, x0, t0, p0, @f_circle, @fp_circle, h, tol );

    if ( status ~= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  STEP failed!\n' );
      break
    end

    xy(1:2,step_num) = x2(1:2,1);
    fx_norm = max ( abs ( f_circle ( n, x2 ) ) );
    fprintf ( 1, '  %2d  %14.6g  %14.6g  %8.2e\n', step_num, x2(1), x2(2), fx_norm );

    if ( p0 ~= p2 )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  Switching parameters from %d to %d\n', p0, p2 );
      p0 = p2;
    end

    x0 = x2;
    t0 = t2;

  end
%
%  Plot the points.
%
  plot ( xy(1,:), xy(2,:), 'r.-', 'Markersize', 25 );
  grid on
  axis equal
  xlabel ( '<--- X --->', 'Fontsize', 16 );
  ylabel ( '<--- Y --->', 'Fontsize', 16 );
  title ( 'Points on the circle, by the continuation method.', 'Fontsize', 24 );

  filename = 'circle_test.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot file saved as "%s"\n', filename );

  return
end


%function s= multivariate_gauss(x,P,n,randstream)
function s= multivariate_gauss(x,P,n)%, randstream
%
% INPUTS:
%   (x, P) mean vector and covariance matrix
%   obtain n samples
% OUTPUT:
%   sample set
%
% Random sample from multivariate Gaussian distribution.
% Adapted from MVNORMRND (c) 1998, Harvard University.
%
% Tim Bailey 2004.

len = length(x);
S   = chol(P)';
%X   = randn(randstream, len,n);
X   = randn( len,n);
s   = S*X + x*ones(1,n);% Generate values from a normal distribution with mean x
%       and standard deviation S.


%LINES5D  Generates three 5-dimensional lines
%
%	A = LINES5D(N);
%
% Generates a data set of N points, on 3 non-crossing, non-parallel lines
% in 5 dimensions.
%
% If N is a vector of sizes, exactly N(I) objects are generated
% for class I, I = 1,2.Default: N = [50 50 50].
%
% See also DATASETS, PRDATASETS

% Copyright: E. Pekalska, R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Sciences, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% $Id: lines5d.m,v 1.2 2006/03/08 22:06:58 duin Exp $

function data = lines5d(N)
			if nargin< 1, N = [50 50 50]; end

	N = genclass(N,ones(1,3)/3);
	n1 = N(1);
	n2 = N(2);
	n3 = N(3);

  s1 = [0 0 0 1 0];
  s2 = [1 1 1 0 0];
  s3 = [0 1 0 1 0];
  s4 = [1 1 1 1 1];
  s5 = [0 1 1 0 1];
  s6 = [1 0 1 1 1];
  c1 = [0:1/(n1-1):1]';
  c2 = [0:1/(n2-1):1]';
  c3 = [0:1/(n3-1):1]';
  a  = c1*s1 + (1-c1)*s2;
  a  = [a; c2*s3 + (1-c2)*s4];
  a  = [a; c3*s5 + (1-c3)*s6];

	data = prdataset(a,genlab(N));
	data = setname(data,'5D Lines');

return


function Lambda = MakeFourierDiagonal_2D(n,deep)
% MakeFourierDiagonal_2D:
%  Usage:
%    Lambda = Inv_SeparateAngles(R,deep,MaxIts,ErrTol);
%  Inputs:
%    n       dyadic integer
%    deep    Depth of the angular splitting
%  Outputs:
%    Lambda   (2*n-1) * n/4 * 2 array of Fourier multipliers
%  Description
%    AtA has a Toeplitz structure. A Toeplitz matrix is embedded in
%    a larger circulant matrix. A circulant matrix is diagonal in a
%    Fourier basis; lambda is the vector of those diagonal
%    coefficients.
%  See Also
%    AtA_Toeplitz, MakeFourierDiagonal
%
% By Emmanuel Candes, 2003-2004

	 n2 = n/2;
         boxlen = n/2^deep;
	 boxcnt = 2^deep;

	 [ix,w] = DetailMeyerWindow([n2/4 n2/2],3);
	 lx = reverse(-ix);

	 m      =  0:(boxcnt - 1);
         ym     =  (m-boxcnt/2).*boxlen + boxlen/2;
	 slope  =  -ym./n2;

	 Lambda = zeros(2*n-1,n2/2,2);

    for r = 1:(n2/2),
      k = lx(r);
      shift  =  ym + slope.*(k+n2);
      alpha =  -k/n2;
      w = MakeSineWindow(boxlen,alpha);
      lambda = MakeFourierDiagonal(n,shift,boxlen,1,w);

      Lambda(:,r,1) = lambda;

      rsym = n2/2 + 1 - r; %shift = -shift + 1;
                           %Lambda(:,rsym,2) = MakeFourierDiagonal(n,shift,boxlen,1,w);
      Lambda(:,rsym,2) =  ifft(conj(fft(lambda)));
    end


function [Aeq beq]= getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond)
    n_all_poly = n_seg*(n_order+1);% the number of all polynomial coefficients
    %#####################################################
    % p,v,a,j constraint in start,
    Aeq_start = zeros(4, n_all_poly);
    beq_start = zeros(4, 1);
    % STEP 2.1: write expression of Aeq_start and beq_start
    Aeq_start(1:1:4, 1:1:n_order+1) = [0, 0, 0, 0, 0, 0, 0, 1;
                                       0, 0, 0, 0, 0, 0, 1, 0;
                                       0, 0, 0, 0, 0, 2, 0, 0;
                                       0, 0, 0, 0, 6, 0, 0, 0];
    beq_start = start_cond';% p,v,a,j

    %#####################################################
    % p,v,a constraint in end
    Aeq_end = zeros(4, n_all_poly);
    beq_end = zeros(4, 1);
    % STEP 2.2: write expression of Aeq_end and beq_end
    T = ts(size(ts,1));% time of the last trajectory
    Aeq_end(1:1:4, n_all_poly-n_order:1:n_all_poly) = [ T^7,     T^6,    T^5,    T^4,   T^3, T^2, T, 1;
                                                      7*T^6,   6*T^5,  5*T^4,  4*T^3, 3*T^2, 2*T, 1, 0;
                                                     42*T^5,  30*T^4, 20*T^3, 12*T^2,   6*T,   2, 0, 0;
                                                    210*T^4, 120*T^3, 60*T^2,   24*T,     6,   0, 0, 0];
    beq_end = end_cond';% p,v,a,j

    %#####################################################
    % position constrain in all middle waypoints
    Aeq_wp = zeros(n_seg-1, n_all_poly);
    beq_wp = zeros(n_seg-1, 1);
    % STEP 2.3: write expression of Aeq_wp and beq_wp
    for midwp_index = 1:n_seg-1
        index = 1 + 8 * (midwp_index - 1);
        T = ts(midwp_index);
        Aeq_wp(midwp_index,index:index+7) = [T^7, T^6, T^5, T^4, T^3, T^2, T, 1];% the end of previous segment
        % Aeq_wp(midwp_index,index+8:index+8+7) = [0, 0, 0, 0, 0, 0, 0, 1];% the begin of next segment
    end
    beq_wp = waypoints(2:n_seg,1);

    %#####################################################
    % position continuity constrain between each 2 segments
    Aeq_con_p = zeros(n_seg-1, n_all_poly);
    beq_con_p = zeros(n_seg-1, 1);
    % STEP 2.4: write expression of Aeq_con_p and beq_con_p
    for con_p_index = 1:n_seg-1
        index = 1 + 8 * (con_p_index - 1);
        T = ts(con_p_index);
        Aeq_con_p(con_p_index,index:index+7) = [T^7, T^6, T^5, T^4, T^3, T^2, T, 1];% the end of previous segment
        Aeq_con_p(con_p_index,index+8:index+8+7) = [0, 0, 0, 0, 0, 0, 0, -1];% the begin of next segment
    end
    % beq_con_p is a zero vector

    %#####################################################
    % velocity continuity constrain between each 2 segments
    Aeq_con_v = zeros(n_seg-1, n_all_poly);
    beq_con_v = zeros(n_seg-1, 1);
    % STEP 2.5: write expression of Aeq_con_v and beq_con_v
    for con_v_index = 1:n_seg-1
        index = 1 + 8 * (con_v_index - 1);
        T = ts(con_v_index);
        Aeq_con_v(con_v_index,index:index+7) = [7*T^6, 6*T^5, 5*T^4, 4*T^3, 3*T^2, 2*T, 1, 0];% the end of previous segment
        Aeq_con_v(con_v_index,index+8:index+8+7) = [0, 0, 0, 0, 0, 0, -1, 0];% the begin of next segment
    end
    % beq_con_v is a zero vector

    %#####################################################
    % acceleration continuity constrain between each 2 segments
    Aeq_con_a = zeros(n_seg-1, n_all_poly);
    beq_con_a = zeros(n_seg-1, 1);
    % STEP 2.6: write expression of Aeq_con_a and beq_con_a
    for con_a_index = 1:n_seg-1
        index = 1 + 8 * (con_a_index - 1);
        T = ts(con_a_index);
        Aeq_con_a(con_a_index,index:index+7) = [42*T^5, 30*T^4, 20*T^3, 12*T^2, 6*T, 2, 0, 0];% the end of previous segment
        Aeq_con_a(con_a_index,index+8:index+8+7) = [0, 0, 0, 0, 0, -2, 0, 0];% the begin of next segment
    end
    % beq_con_a is a zero vector

    %#####################################################
    % jerk continuity constrain between each 2 segments
    Aeq_con_j = zeros(n_seg-1, n_all_poly);
    beq_con_j = zeros(n_seg-1, 1);
    % STEP 2.7: write expression of Aeq_con_j and beq_con_j
    for con_j_index = 1:n_seg-1
        index = 1 + 8 * (con_j_index - 1);
        T = ts(con_j_index);
        Aeq_con_j(con_j_index,index:index+7) = [210*T^4, 120*T^3, 60*T^2, 24*T, 6, 0, 0, 0];% the end of previous segment
        Aeq_con_j(con_j_index,index+8:index+8+7) = [0, 0, 0, 0, -6, 0, 0, 0];% the begin of next segment
    end
    % beq_con_j is a zero vector

    %#####################################################
    % combine all components to form Aeq and beq
    Aeq_con = [Aeq_con_p; Aeq_con_v; Aeq_con_a; Aeq_con_j];
    beq_con = [beq_con_p; beq_con_v; beq_con_a; beq_con_j];
    Aeq = [Aeq_start; Aeq_end; Aeq_wp; Aeq_con];
    beq = [beq_start; beq_end; beq_wp; beq_con];
end


%DISTM Compute square Euclidean distance matrix
%
%   D = DISTM(A,B)
%
% INPUT
%   A,B   Datasets or matrices; B is optional, default B = A
%
% OUTPUT
%   D     Square Euclidean distance dataset or matrix
%
% DESCRIPTION
% Computation of the square Euclidean distance matrix D between two
% sets A and B. If A has M objects and B has N objects, then D is
% [M x N]. If A and B are datasets, then D is a dataset as well with
% the labels defined by the labels of A and the feature labels defined
% by the labels of B.
%
% Unlabeled objects in B are neglected, unless B is entirely unlabeled.
%
% If A is not a dataset, but a matrix of doubles then D is also not a
% dataset, but a set of doubles.
%
% NOTE
% DISTM(A,B) is equivalent to A*PROXM(B,'d',2)).
%
% SEE ALSO
% DATASETS, PROXM

% Copyright: R.P.W. Duin, r.p.w.duin@prtools.org
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: distm.m,v 1.9 2006/03/07 16:14:41 duin Exp $

function D = distm_new(A,B,DirectionalFeatures)
	prtrace(mfilename);
	if nargin < 2
		B = A;
	end
	B = cdats(B,1);
	[ma,ka] = size(A);
	[mb,kb] = size(B);

	if (ka ~= kb)
		error('Feature sizes should be equal')
    end

    if( isempty(DirectionalFeatures) )
        % The order of operations below is good for the accuracy.
        D = ones(ma,1)*sum(B'.*B',1);
        D = D + sum(A'.*A',1)'*ones(1,mb);
        D = D - 2 .* (+A)*(+B)';
    else

        %simulo los dos wraps posibles
        B1 = B(:,DirectionalFeatures) + 2*pi;
        B2 = B(:,DirectionalFeatures) - 2*pi;

        %mido las distancias con y sin los 2 wraps
        D = ones(ma,1)*sum(B'.*B',1);
        D = D + sum(A'.*A',1)'*ones(1,mb);
        D = D - 2 .* (+A)*(+B)';

        D1 = ones(ma,1)*sum(B1'.*B1',1);
        D1 = D1 + sum(A'.*A',1)'*ones(1,mb);
        D1 = D1 - 2 .* (+A)*(+B1)';

        D2 = ones(ma,1)*sum(B2'.*B2',1);
        D2 = D2 + sum(A'.*A',1)'*ones(1,mb);
        D2 = D2 - 2 .* (+A)*(+B2)';

        %me quedo con la menor distancia.
        D = reshape(min([ D(:) D1(:) D2(:) ],[],2), ma, ma);

    end

	J = find(D<0);                  % Check for a numerical inaccuracy.
	D(J) = zeros(size(J));          % D should be nonnegative.

	if ((nargin < 2) & (ma == mb)) % take care of symmetric distance matrix
		D = (D + D')/2;
		D([1:ma+1:ma*ma]) = zeros(1,ma);
	end

	if isa(A,'prdataset')   % set object and feature labels
		if isa(B,'prdataset')
			D = setdata(A,D,getlab(B));
		else
			D = setdata(A,D);
		end
	end

return


function f = p06_fun ( option, n, x )

%*****************************************************************************80
%
%% P06_FUN evaluates the integrand for problem 6.
%
%  Discussion:
%
%    The exact value is (m-1)!! * sqrt ( pi ) / sqrt ( 2**m ).
%
%    Integral ( -oo < x < +oo ) x^m exp (-x*x) dx
%
%    The parameter M is set by calling P06_PARAM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 May 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer OPTION:
%    0, integrand is f(x).
%    1, integrand is exp(-x*x) * f(x);
%    2, integrand is exp(-x*x/2) * f(x);
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real F(N), the function values.
%
  x = x ( : );
  f = zeros ( n, 1 );

  m = 0;
  m = p06_param ( 'G', 'M', m );

  f(1:n) = x(1:n).^m;

  if ( option == 0 )
    f(1:n) = f(1:n) .* exp ( - x(1:n).^2 );
  elseif ( option == 1 )

  elseif ( option == 2 )
    f(1:n) = f(1:n) .* exp ( - 0.5 * x(1:n).^2 );
  end

  return
end


%JTRAJ Compute a joint space trajectory between two configurations
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, M) is a joint space trajectory Q (MxN) where the joint
% coordinates vary from Q0 (1xN) to QF (1xN).  A quintic (5th order) polynomial is used
% with default zero boundary conditions for velocity and acceleration.
% Time is assumed to vary from 0 to 1 in M steps.  Joint velocity and
% acceleration can be optionally returned as QD (MxN) and QDD (MxN) respectively.
% The trajectory Q, QD and QDD are MxN matrices, with one row per time step,
% and one column per joint.
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, M, QD0, QDF) as above but also specifies initial
% and final joint velocity for the trajectory.
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, T) as above but the trajectory length is defined
% by the length of the time vector T (Mx1).
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, T, QD0, QDF) as above but specifies initial and
% final joint velocity for the trajectory and a time vector.
%
% See also QPLOT, CTRAJ, SerialLink.jtraj.

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

function [qt,qdt,qddt] = jtraj(q0, q1, tv, qd0, qd1)
    if length(tv) > 1
        tscal = max(tv);
        t = tv(:)/tscal;
    else
        tscal = 1;
        t = (0:(tv-1))'/(tv-1); % normalized time from 0 -> 1
    end

    q0 = q0(:);
    q1 = q1(:);

    if nargin == 3
        qd0 = zeros(size(q0));
        qd1 = qd0;
    elseif nargin == 5
        qd0 = qd0(:);
        qd1 = qd1(:);
    else
        error('incorrect number of arguments')
    end

    % compute the polynomial coefficients
    A = 6*(q1 - q0) - 3*(qd1+qd0)*tscal;
    B = -15*(q1 - q0) + (8*qd0 + 7*qd1)*tscal;
    C = 10*(q1 - q0) - (6*qd0 + 4*qd1)*tscal;
    E = qd0*tscal; % as the t vector has been normalized
    F = q0;

    tt = [t.^5 t.^4 t.^3 t.^2 t ones(size(t))];
    c = [A B C zeros(size(A)) E F]';

    qt = tt*c;

    % compute optional velocity
    if nargout >= 2
        c = [ zeros(size(A)) 5*A 4*B 3*C  zeros(size(A)) E ]';
        qdt = tt*c/tscal;
    end

    % compute optional acceleration
    if nargout == 3
        c = [ zeros(size(A))  zeros(size(A)) 20*A 12*B 6*C  zeros(size(A))]';
        qddt = tt*c/tscal^2;
    end