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 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 [ 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


%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


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


function Wrot = whiteningFromCovariance(CC)
% takes as input the matrix CC of channel pairwise correlations
% outputs a symmetric rotation matrix (also Nchan by Nchan) that rotates
% the data onto uncorrelated, unit-norm axes

[E, D] 	= svd(CC); % covariance eigendecomposition (same as svd for positive-definite matrix)
D       = diag(D); % take the non-zero values from the diagonal
eps 	= 1e-6;
Wrot 	= E * diag(1./(D + eps).^.5) * E'; % this is the symmetric whitening matrix (ZCA transform)


function J = ObjectiveFCN(u,x,Ts,N,Nu,xref,u0,p,Q,R,Ru)
%% Cost function of nonlinear MPC for Lotka-Volterra system
%
% Inputs:
%   u:      optimization variable, from time k to time k+N-1
%   x:      current state at time k
%   Ts:     controller sample time
%   N:      prediction horizon
%   Nu:     control horizon [not implemented]
%   xref:   state references, varying from time k+1 to k+N
%   u0:     previous controller output at time k-1
%    p:      Parameters for model
%    Q:      State weights
%    R:      Penalization/weights on rate of change in u, uk - uk-1
%    Ru:     Control input u penalization/weights
%
% Output:
%   J:      objective function cost
%

%% Nonlinear MPC design parameters

%% Cost Calculation
% Set initial plant states, controller output and cost
xk = x;
uk = u(1);
J = 0;

% Loop through each prediction step
for ct=1:N

    % Obtain plant state at next prediction step
    xk1 = rk4u(@F8Sys,xk,uk,Ts,1,[],p);

    % Accumulate state tracking cost from x(k+1) to x(k+N)
    J = J + (xk1-xref(:,ct))'*Q*(xk1-xref(:,ct));

    % Accumulate rate of change cost from u(k) to u(k+N-1)
    if ct==1
        J = J + (uk-u0)'*R*(uk-u0) + uk'*Ru*uk;
    else
        J = J + (uk-u(ct-1))'*R*(uk-u(ct-1)) + uk'*Ru*uk;
    end

    % Update xk and uk for the next prediction step
    xk = xk1;
    if ct<N
        uk = u(ct+1);
    end
end


function [r] = volume2radius(V)
% volume2radius returns the radius of a sphere of volume V.
% Chad Greene 2012
r = (3*V/(4*pi)).^(1/3);


function ng = triangle_grid_count ( n )

%*****************************************************************************80
%
%% TRIANGLE_GRID_COUNT counts the grid points inside a triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of subintervals.
%
%    Output, integer NG, the number of grid points inside the triangle.
%
  ng = ( ( n + 1 ) * ( n + 2 ) ) / 2;

  return
end


function [logCount,t] = mandelbrotViewerProcessElement( x0, y0, escapeRadius2, maxIterations )
% Evaluate the Mandelbrot function for a single element

%   Copyright 2010-2011 The Mathworks, Inc.

t = 1;
z0 = complex( x0, y0 );
z = z0;
count = 0;
while count <= maxIterations && (z*conj(z) <= escapeRadius2)
    z = z*z + z0;
    count = count + 1;
end
magZ2 = max(real(z).^2 + imag(z).^2,escapeRadius2);
logCount = log( count + 1 - log( log( magZ2 ) / 2 ) / log(2) );


function value = i4_log_r8 ( x, b )

%*****************************************************************************80
%
%% I4_LOG_R8 returns the integer part of the logarithm base ABS(B) of ABS(X).
%
%  Example:
%
%    If B is greater than 1, and X is positive:
%
%    if 1/B^2  <  X <= 1/B   I4_LOG_R8(X) = -1,
%    if 1/B    <  X <= 1     I4_LOG_R8(X) = 0,
%    if 1      <= X <  B,    I4_LOG_R8(X) = 0,
%    if B      <= X <  B^2   I4_LOG_R8(X) = 1,
%    if B^2    <= X <  B^3   I4_LOG_R8(X) = 2.
%
%    For positive I4_LOG_R8(X), it should be true that
%
%      ABS(B)^I4_LOG_R8(X) <= ABS(X) < ABS(B)^(I4_LOG_R8(X)+1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer X, the number whose logarithm base B is desired.
%    If X is 0, then I4_LOG_R8 is returned as -HUGE().
%
%    Input, real B, the absolute value of the base of the
%    logarithms.  B must not be -1, 0, or 1.
%
%    Output, integer VALUE, the integer part of the logarithm
%    base abs(B) of X.
%
  i4_huge = 2147483647;

  x = floor ( x );

  if ( x == 0 )
    value = - i4_huge;
    return
  end

  b = abs ( b );
  value = 0;

  if ( b == 1.0 )
    return
  end

  if ( b == 0.0 )
    return
  end

  x = abs ( x );

  if ( b < 1.0 )
    value_sign = -1;
    b = 1.0 / b;
  else
    value_sign = +1;
  end

  if ( 1.0 <= x && x < b )
    value = value_sign * value;
    return
  end

  while ( b < x )
    x = x / b;
    value = value + 1;
  end

  while ( x * b <= 1.0 )
    x = x * b;
    value = value - 1;
  end
%
%  If the absolute value of the base was less than 1, we inverted
%  earlier.  Now negate the logarithm to account for that.
%
  value = value_sign * value;

  return
end


function U = fda(X, t, q)
% Fisher (linear) discriminant analysis
% Input:
%   X: d x n data matrix
%   t: 1 x n class label
%   d: target dimension
% Output:
%   U: projection matrix y=U'*x
% Written by Mo Chen (sth4nth@gmail.com).
n = size(X,2);
k = max(t);

E = sparse(1:n,t,true,n,k,n);  % transform label into indicator matrix
nk = full(sum(E));

m = mean(X,2);
Xo = bsxfun(@minus,X,m);
St = (Xo*Xo')/n;                   % 4.43

mk = bsxfun(@times,X*E,1./nk);
mo = bsxfun(@minus,mk,m);
mo = bsxfun(@times,mo,sqrt(nk/n));
Sb = mo*mo';                       % 4.46
% Sw = St-Sb;                        % 4.45

[U,A] = eig(Sb,St,'chol');
[~,idx] = sort(diag(A),'descend');
U = U(:,idx(1:q));


% DIRECT JACOBIAN DEMO
function cinematica_inversa_3gdl
% definimos la posición y orientación del extremo en 2D
phi = pi/8
px = 2
py = 2
% definimos una matriz T
T = [cos(phi) -sin(phi) 0 px;
     sin(phi) cos(phi)  0 py;
     0 0 1 0;
     0 0 0 1];

 q = ikine3DOF(T)

 function q = ikine3DOF(T)
 L1 = 1;
 L2 = 1;
 L3 = 1; % 1m, parámetro del robot

 % hallamos pm
 p = T(1:3, 4);

%find angle Phi
x3 = T(1:3,1);

cphi = x3'*[1 0 0]';
sphi = x3'*[0 1 0]';
phi = atan2(sphi, cphi);

% pm
pm = p - L3*x3

%Distance of the point to the origin.
R= sqrt(pm(1)^2+pm(2)^2);

if R > (L1+L2)
   disp('\ninversekinematic_3dofplanar: unfeasible solution. The point cannot be reached');
end

%compute geometric solution
beta = atan2(pm(2),pm(1));
gamma = real(acos((L1^2+R^2-L2^2)/(2*R*L1)));
delta = real(acos((L1^2+L2^2-R^2)/(2*L1*L2)));

%arrange possible combinations for q(1) and q(2)
%elbow down     elbow up solutions
q =[beta+gamma beta-gamma;
    delta-pi   pi-delta];

%in this case, phi = q(1) + q(2) + q(3) and
%q(3) can be computed as q(3) = phi - q(1) - q(2)
%corresponding to each of the previous solutions, a unique q(3) can be
%computed for each case
for i=1:2 %iterate through columns
    q(3,i) = phi - q(1,i) - q(2,i);
end


function Rx=correlmx(x,p,Rxtype);
% Rx=correlmx(x,p,Rxtype) correlation matrix of a signal
%
%        Rx : correlation matrix (p+1) x (p+1)
%         x : analyzed signal
%         p : last autocorrelation lag
%    Rxtype : computation algorithm (default : 'fbhermitian')
%             possible values : 'hermitian', 'fbhermitian', 'burg' or 'fbburg'
%
% example :
%
% N=100; sig=real(fmconst(N,0.1))+0.4*randn(N,1);
% Rx=correlmx(sig,2,'burg'); [v,d] = eig(Rx), acos(-0.5*v(2,1)/v(1,1))/(2*pi)
% Rx=correlmx(sig,2,'hermitian'); [v,d] = eig(Rx), acos(-0.5*v(2,1)/v(1,1))/(2*pi)

% F. Auger, july 1998.

if (nargin<2),
 error('At least two parameters required');
elseif (nargin==2),
 Rxtype='fbhermitian';
end;

[L,xcol]=size(x);
if xcol>1,
 error('x must be a column vector');
elseif p>L,
 error('L must be greater than p');
elseif p<1,
 error('p must be greater than 0');
end;

Rxtype=upper(Rxtype);
if strcmp(Rxtype,'HERMITIAN')|strcmp(Rxtype,'FBHERMITIAN'),
 vector=x(p+1-(0:p)); Rx=conj(vector) * vector.';
 for t=p+2:L,
  vector=x(t-(0:p)); Rx=Rx+conj(vector) * vector.';
 end;
 Rx=Rx/(L-p);

elseif strcmp(Rxtype,'BURG')|strcmp(Rxtype,'FBBURG'),
 R0=sum(abs(x).^2)/L; % variance
 Rpos=zeros(1,p); Rneg=zeros(1,p);
 for n=1:p,
  Rpos(n)=sum(x(n+1:L).*conj(x(1:L-n)))/(L-n);
  Rneg(n)=sum(x(1:L-n).*conj(x(n+1:L)))/(L-n);
 end;
 Rx=toeplitz([R0 Rpos],[R0 Rneg]);
else error(['unknown algorithm name' Rxtype]);
end;

if strcmp(Rxtype(1:2),'FB'),
 Rx=0.5*(Rx+Rx');
end;
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA


function [ o, c, e ] = lpp_to_polynomial ( m, l, o_max )

%*****************************************************************************80
%
%% LPP_TO_POLYNOMIAL writes a Legendre Product Polynomial as a polynomial.
%
%  Discussion:
%
%    For example, if
%      M = 3,
%      L = ( 1, 0, 2 ),
%    then
%      L(1,0,2)(X,Y,Z)
%      = L(1)(X) * L(0)(Y) * L(2)(Z)
%      = X * 1 * ( 3Z^2-1)/2
%      = - 1/2 X + (3/2) X Z^2
%    so
%      O = 2 (2 nonzero terms)
%      C = -0.5
%           1.5
%      E = 4    <-- index in 3-space of exponent (1,0,0)
%          15   <-- index in 3-space of exponent (1,0,2)
%
%    The output value of O is no greater than
%      O_MAX = product ( 1 <= I <= M ) (L(I)+2)/2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer L(M), the index of each Legendre product polynomial factor.
%    0 <= L(*).
%
%    Input, integer O_MAX, an upper limit on the size of the output arrays.
%      O_MAX = product ( 1 <= I <= M ) (L(I)+2)/2.
%
%    Output, integer O, the "order" of the polynomial product.
%
%    Output, real C(O), the coefficients of the polynomial product.
%
%    Output, integer E(O), the indices of the exponents of the
%    polynomial product.
%
  o1 = 1;
  c1 = 1.0;
  e1 = 1;
%
%  Implicate one factor at a time.
%
  for i = 1 : m

    [ o2, c2, f2 ] = lp_coefficients ( l(i) );

    o = 0;

    for j2 = 1 : o2
      for j1 = 1 : o1
        o = o + 1;
        c(o) = c1(j1) * c2(j2);

        if ( 1 < i )
          p = mono_unrank_grlex ( i - 1, e1(j1) );
        end
        p(i) = f2(j2);
        e(o) = mono_rank_grlex ( i, p );

      end
    end

    [ c, e ] = polynomial_sort ( o, c, e );
    [ o, c, e ] = polynomial_compress ( o, c, e );

    o1 = o;
    c1 = c;
    e1 = e;

  end

  return
end


function fem1d ( )

%*****************************************************************************80
%
%% MAIN is the main program for FEM1D.
%
%  Discussion:
%
%    FEM1D solves a one dimensional ODE using the finite element method.
%
%    The differential equation has the form:
%
%      -d/dx ( p(x) du/dx ) + q(x) * u = f(x)
%
%    The finite-element method uses piecewise linear basis functions.
%
%    Here U is an unknown scalar function of X defined on the
%    interval [XL,XR], and P, Q and F are given functions of X.
%
%    The values of U or U' at XL and XR are also specified.
%
%    The interval [XL,XR] is "meshed" with NSUB+1 points,
%
%    XN(0) = XL, XN(1)=XL+H, XN(2)=XL+2*H, ..., XN(NSUB)=XR.
%
%    This creates NSUB subintervals, with interval number 1
%    having endpoints XN(0) and XN(1), and so on up to interval
%    NSUB, which has endpoints XN(NSUB-1) and XN(NSUB).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 October 2008
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    real ADIAG(NU).
%    ADIAG(I) is the "diagonal" coefficient of the I-th
%    equation in the linear system.  That is, ADIAG(I) is
%    the coefficient of the I-th unknown in the I-th equation.
%
%    real ALEFT(NU).
%    ALEFT(I) is the "left hand" coefficient of the I-th
%    equation in the linear system.  That is, ALEFT(I) is the
%    coefficient of the (I-1)-th unknown in the I-th equation.
%    There is no value in ALEFT(1), since the first equation
%    does not refer to a "0-th" unknown.
%
%    real ARITE(NU).
%    ARITE(I) is the "right hand" coefficient of the I-th
%    equation in the linear system.  ARITE(I) is the coefficient
%    of the (I+1)-th unknown in the I-th equation.  There is
%    no value in ARITE(NU) because the NU-th equation does not
%    refer to an "NU+1"-th unknown.
%
%    real F(NU).
%    ASSEMBLE stores into F the right hand side of the linear
%    equations.
%    SOLVE replaces those values of F by the solution of the
%    linear equations.
%
%    real H(N), the length of the subintervals.
%
%    integer IBC. declares what the boundary conditions are.
%    1, at the left endpoint, U has the value UL,
%       at the right endpoint, U' has the value UR.
%    2, at the left endpoint, U' has the value UL,
%       at the right endpoint, U has the value UR.
%    3, at the left endpoint, U has the value UL,
%       and at the right endpoint, U has the value UR.
%    4, at the left endpoint, U' has the value UL,
%       at the right endpoint U' has the value UR.
%
%    integer INDX(1:N+1).
%    For a node I, INDX(I) is the index of the unknown
%    associated with node I.
%    If INDX(I) is equal to -1, then no unknown is associated
%    with the node, because a boundary condition fixing the
%    value of U has been applied at the node instead.
%    Unknowns are numbered beginning with 1.
%    If IBC is 2 or 4, then there is an unknown value of U
%    at node 0, which will be unknown number 1.  Otherwise,
%    unknown number 1 will be associated with node 1.
%    If IBC is 1 or 4, then there is an unknown value of U
%    at node N, which will be unknown N or N+1,
%    depending on whether there was an unknown at node 0.
%
%    integer NL, the number of basis functions used in a single
%    subinterval.  (NL-1) is the degree of the polynomials
%    used.  For this code, NL is fixed at 2, meaning that
%    piecewise linear functions are used as the basis.
%
%    integer NODE(NL,N).
%    For each subinterval I:
%    NODE(1,I) is the number of the left node, and
%    NODE(2,I) is the number of the right node.
%
%    integer NQUAD.
%    The number of quadrature points used in a subinterval.
%    This code uses NQUAD = 1.
%
%    integer NSUB, the number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    integer NU, the number of unknowns in the linear system.
%    Depending on the value of IBC, there will be N-1,
%    N, or N+1 unknown values, which are the coefficients
%    of basis functions.
%
%    real UL.
%    If IBC is 1 or 3, UL is the value that U is required
%    to have at X = XL.
%    If IBC is 2 or 4, UL is the value that U' is required
%    to have at X = XL.
%
%    real UR.
%    If IBC is 2 or 3, UR is the value that U is required
%    to have at X = XR.
%    If IBC is 1 or 4, UR is the value that U' is required
%    to have at X = XR.
%
%    real XL, the left endpoint of the interval over which the
%    differential equation is being solved.
%
%    real XN(1:N+1).
%    XN(I) is the location of the I-th node.  XN(1) is XL,
%    and XN(N+1) is XR.
%
%    real XQUAD(N)
%    XQUAD(I) is the location of the single quadrature point
%    in interval I.
%
%    real XR, the right endpoint of the interval over which the
%    differential equation is being solved.
%
  n = 100;
  nl = 2;

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the two-point boundary value problem:\n' );
  fprintf ( 1, '    -d/dx (p(x) du/dx) + q(x)*u  =  f(x)\n' );
  fprintf ( 1, '  on an interval [xl,xr], with the values of\n' );
  fprintf ( 1, '  u or u'' specified at xl and xr.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The interval is broken into %d subintervals.\n', n );
  fprintf ( 1, '  The number of basis functions per element is %d\n', nl );
%
%  Initialize variables that define the problem.
%
  [ ibc, nquad, ul, ur, xl, xr ] = init ( );
%
%  Compute the quantities that describe the geometry of the problem.
%
  [ h, indx, node, nu, xn, xquad ] = geometry ( ibc, nl, n, xl, xr );
%
%  Assemble the matrix.
%
  [ adiag, aleft, arite, f ] = assemble ( h, indx, nl, node, ...
    nu, nquad, n, ul, ur, xn, xquad );
%
%  Print out the linear system.
%
  system_print ( adiag, aleft, arite, f, nu );
%
%  Solve the linear system.
%
  u = solve ( adiag, aleft, arite, f, nu );
%
%  Print the current solution.
%
  output ( u, ibc, indx, n, nu, ul, ur, xn );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function [ adiag, aleft, arite, f ] = assemble ( h, indx, nl, ...
  node, nu, nquad, n, ul, ur, xn, xquad )

%*****************************************************************************80
%
%% ASSEMBLE assembles the matrix and right hand side of the linear system.
%
%  Discussion:
%
%    Note that a 1 point quadrature rule, which is sometimes used to
%    assemble the matrix and right hand side, is just barely accurate
%    enough for simple problems.  If you want better results, you
%    should use a quadrature rule that is more accurate.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real H(N), the length of the subintervals.
%
%    Input, integer INDX(1:N+1).
%    For a node I, INDX(I) is the index of the unknown
%    associated with node I.
%    If INDX(I) is equal to -1, then no unknown is associated
%    with the node, because a boundary condition fixing the
%    value of U has been applied at the node instead.
%    Unknowns are numbered beginning with 1.
%    If IBC is 2 or 4, then there is an unknown value of U
%    at node 0, which will be unknown number 1.  Otherwise,
%    unknown number 1 will be associated with node 1.
%    If IBC is 1 or 4, then there is an unknown value of U
%    at node N, which will be unknown N or N+1,
%    depending on whether there was an unknown at node 0.
%
%    Input, integer NL.
%    The number of basis functions used in a single
%    subinterval.  (NL-1) is the degree of the polynomials
%    used.  For this code, NL is fixed at 2, meaning that
%    piecewise linear functions are used as the basis.
%
%    Input, integer NODE(NL,N).
%    For each subinterval I:
%    NODE(1,I) is the number of the left node, and
%    NODE(2,I) is the number of the right node.
%
%    Input, integer NU.
%    NU is the number of unknowns in the linear system.
%    Depending on the value of IBC, there will be N-1,
%    N, or N+1 unknown values, which are the coefficients
%    of basis functions.
%
%    Input, integer NQUAD, the number of quadrature points in a subinterval.
%    This code uses NQUAD = 1.
%
%    Input, integer N, the number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    Input, real UL.
%    If IBC is 1 or 3, UL is the value that U is required
%    to have at X = XL.
%    If IBC is 2 or 4, UL is the value that U' is required
%    to have at X = XL.
%
%    Input, real UR.
%    If IBC is 2 or 3, UR is the value that U is required
%    to have at X = XR.
%    If IBC is 1 or 4, UR is the value that U' is required
%    to have at X = XR.
%
%    Input, real XN(1:N+1), the location of the I-th node.  XN(1) is XL,
%    and XN(N+1) is XR.
%
%    Input, real XQUAD(N), the location of the single quadrature point
%    in interval I.
%
%    Output, real ADIAG(NU).
%    ADIAG(I) is the "diagonal" coefficient of the I-th
%    equation in the linear system.  That is, ADIAG(I) is
%    the coefficient of the I-th unknown in the I-th equation.
%
%    Output, real ALEFT(NU).
%    ALEFT(I) is the "left hand" coefficient of the I-th
%    equation in the linear system.  That is, ALEFT(I) is the
%    coefficient of the (I-1)-th unknown in the I-th equation.
%    There is no value in ALEFT(1), since the first equation
%    does not refer to a "0-th" unknown.
%
%    Output, real ARITE(NU).
%    ARITE(I) is the "right hand" coefficient of the I-th
%    equation in the linear system.  ARITE(I) is the coefficient
%    of the (I+1)-th unknown in the I-th equation.  There is
%    no value in ARITE(NU) because the NU-th equation does not
%    refer to an "NU+1"-th unknown.
%
%    Output, real F(NU), the right hand side of the linear
%    equations.
%
  f(1:nu) = 0.0;
  adiag(1:nu) = 0.0;
  aleft(1:nu) = 0.0;
  arite(1:nu) = 0.0;
%
%  For element IE...
%
  for ie = 1 : n

    he = h(ie);
    xleft = xn(node(1,ie)+1);
    xrite = xn(node(2,ie)+1);
%
%  For quadrature point IQ...
%
    for iq = 1 : nquad

      xqe = xquad(ie);
%
%  For basis function IL...
%
      for il = 1 : nl

        ig = node(il,ie);
        iu = indx(ig+1);

        if ( 0 < iu )

          [ phii, phiix ] = phi ( il, xqe, xleft, xrite );

          f(iu) = f(iu) + he * ff ( xqe ) * phii;
%
%  Handle boundary conditions.
%
          if ( ig == 0 )

            x = xn(1);
            f(iu) = f(iu) - pp ( x ) * ul;

          elseif ( ig == n )

            x = xn(n+1);
            f(iu) = f(iu) + pp ( x ) * ur;

          end
%
%  For basis function JL...
%
          for jl = 1 : nl

            jg = node(jl,ie);
            ju = indx(jg+1);

            [ phij, phijx ] = phi ( jl, xqe, xleft, xrite );

            aij = he * ( pp ( xqe ) * phiix * phijx ...
                  + qq ( xqe ) * phii * phij );

            if ( ju <= 0 )

              if ( jg == 0 )
                f(iu) = f(iu) - aij * ul;
              elseif ( jg == n )
                f(iu) = f(iu) - aij * ur;
              end

            elseif ( iu == ju )
              adiag(iu) = adiag(iu) + aij;
            elseif ( ju < iu )
              aleft(iu) = aleft(iu) + aij;
            else
              arite(iu) = arite(iu) + aij;
            end

          end

        end

      end

    end

  end

  return
end
function value = ff ( x )

%*****************************************************************************80
%
%% FF returns the right hand side of the differential equation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of F(X).
%
  value = 0.0;

  return
end
function [ h, indx, node, nu, xn, xquad ] = geometry ( ibc, nl, nsub, ...
  xl, xr )

%*****************************************************************************80
%
%% GEOMETRY sets up the geometry for the interval [XL,XR].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer IBC.
%    IBC declares what the boundary conditions are.
%    1, at the left endpoint, U has the value UL,
%       at the right endpoint, U' has the value UR.
%    2, at the left endpoint, U' has the value UL,
%       at the right endpoint, U has the value UR.
%    3, at the left endpoint, U has the value UL,
%       and at the right endpoint, U has the value UR.
%    4, at the left endpoint, U' has the value UL,
%       at the right endpoint U' has the value UR.
%
%    Input, integer NL.
%    The number of basis functions used in a single
%    subinterval.  (NL-1) is the degree of the polynomials
%    used.  For this code, NL is fixed at 2, meaning that
%    piecewise linear functions are used as the basis.
%
%    Input, integer NSUB.
%    The number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    Input, real XL.
%    XL is the left endpoint of the interval over which the
%    differential equation is being solved.
%
%    Input, real XR.
%    XR is the right endpoint of the interval over which the
%    differential equation is being solved.
%
%    Output, real H(NSUB), the length of the subintervals.
%
%    Output, integer INDX(1:NSUB+1).
%    For a node I, INDX(I) is the index of the unknown
%    associated with node I.
%    If INDX(I) is equal to -1, then no unknown is associated
%    with the node, because a boundary condition fixing the
%    value of U has been applied at the node instead.
%    Unknowns are numbered beginning with 1.
%    If IBC is 2 or 4, then there is an unknown value of U
%    at node 0, which will be unknown number 1.  Otherwise,
%    unknown number 1 will be associated with node 1.
%    If IBC is 1 or 4, then there is an unknown value of U
%    at node N, which will be unknown N or N+1,
%    depending on whether there was an unknown at node 0.
%
%    Output, integer NODE(NL,NSUB).
%    For each subinterval I:
%    NODE(1,I) is the number of the left node, and
%    NODE(2,I) is the number of the right node.
%
%    Output, integer NU.
%    NU is the number of unknowns in the linear system.
%    Depending on the value of IBC, there will be NSUB-1,
%    NSUB, or NSUB+1 unknown values, which are the coefficients
%    of basis functions.
%
%    Output, real XN(1:NSUB+1).
%    XN(I) is the location of the I-th node.  XN(1) is XL,
%    and XN(N+1) is XR.
%
%    Output, real XQUAD(NSUB)
%    XQUAD(I) is the location of the single quadrature point
%    in interval I.
%

%
%  Set the value of XN, the locations of the nodes.
%
  xn = zeros ( nsub + 1, 1 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Node      Location\n' );
  fprintf ( 1, '\n' );
  for i = 0 : nsub
    xn(i+1) =  ( ( nsub - i ) * xl   ...
               + (        i ) * xr ) ...
               / ( nsub     );
  end
  r8vec_print_some ( nsub + 1, xn, 1, 10, '  First 10 nodes:' );
%
%  Set the lengths of each subinterval.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Subint    Length\n' );
  fprintf ( 1, '\n' );
  h = zeros ( nsub, 1 );
  for i = 1 : nsub
    h(i) = xn(i+1) - xn(i);
  end
  r8vec_print_some ( nsub, h, 1, 10, '  First 10 interval widths:' );
%
%  Set the quadrature points, each of which is the midpoint of its subinterval.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Subint    Quadrature point\n' );
  fprintf ( 1, '\n' );
  xquad = zeros ( nsub );
  for i = 1 : nsub
    xquad(i) = 0.5 * ( xn(i) + xn(i+1) );
  end
  r8vec_print_some ( nsub, xquad, 1, 10, '  First 10 quadrature points:' );
%
%  Set the value of NODE, which records, for each interval,
%  the node numbers at the left and right.
%
  node = zeros ( 2, nsub );
  for i = 1 : nsub
    node(1,i) = i - 1;
    node(2,i) = i;
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  First 10 pairs of nodes defining intervals:\n' );
  fprintf ( 1, 'Subint  Left Node  Right Node\n' );
  fprintf ( 1, '\n' );
  for i = 1 : min ( nsub, 10 )
    fprintf ( 1, '  %6d  %6d  %6d\n', i, node(1,i), node(2,i) );
  end
%
%  Starting with node 0, see if an unknown is associated with
%  the node.  If so, give it an index.
%
  nu = 0;
%
%  Handle first node.
%
  i = 0;
  if ( ibc == 1 || ibc == 3 )
    indx(i+1) = -1;
  else
    nu = nu + 1;
    indx(i+1) = nu;
  end
%
%  Handle nodes 1 through nsub-1
%
  for i = 1 : nsub-1
    nu = nu + 1;
    indx(i+1) = nu;
  end
%
%  Handle the last node.
%
  i = nsub;
  if ( ibc == 2 || ibc == 3 )
    indx(i+1) = -1;
  else
    nu = nu + 1;
    indx(i+1) = nu;
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  First 10 unknown indices\n' );
  fprintf ( 1, '    Node  Unknown\n' );
  fprintf ( 1, '\n' );
  for i = 0 : min ( nsub, 9 )
    fprintf ( 1, '  %6d  %6d\n', i, indx(i+1) );
  end

  return
end
function [ ibc, nquad, ul, ur, xl, xr ] = init  ( )

%*****************************************************************************80
%
%% INIT initializes variables that define the problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Output, integer IBC.
%    IBC declares what the boundary conditions are.
%    1, at the left endpoint, U has the value UL,
%       at the right endpoint, U' has the value UR.
%    2, at the left endpoint, U' has the value UL,
%       at the right endpoint, U has the value UR.
%    3, at the left endpoint, U has the value UL,
%       and at the right endpoint, U has the value UR.
%    4, at the left endpoint, U' has the value UL,
%       at the right endpoint U' has the value UR.
%
%    Output, integer NQUAD.
%    The number of quadrature points used in a subinterval.
%    This code uses NQUAD = 1.
%
%    Output, real UL.
%    If IBC is 1 or 3, UL is the value that U is required
%    to have at X = XL.
%    If IBC is 2 or 4, UL is the value that U' is required
%    to have at X = XL.
%
%    Output, real UR.
%    If IBC is 2 or 3, UR is the value that U is required
%    to have at X = XR.
%    If IBC is 1 or 4, UR is the value that U' is required
%    to have at X = XR.
%
%    Output, real XL.
%    XL is the left endpoint of the interval over which the
%    differential equation is being solved.
%
%    Output, real XR.
%    XR is the right endpoint of the interval over which the
%    differential equation is being solved.
%
  ibc = 1;
  nquad = 1;
  ul = 0.0;
  ur = 1.0;
  xl = 0.0;
  xr = 1.0;
%
%  Print out the values that have been set.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'The equation is to be solved for\n' );
  fprintf ( 1, 'X greater than XL = %f\n', xl );
  fprintf ( 1, ' and less than XR = %f\n', xr );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'The boundary conditions are:\n' );
  fprintf ( 1, '\n' );

  if ( ibc == 1 || ibc == 3 )
    fprintf ( 1, '  At X = XL, U = %f\n', ul );
  else
    fprintf ( 1, '  At X = XL, U'' = %f\n', ul );
  end

  if ( ibc == 2 || ibc == 3 )
    fprintf ( 1, '  At X = XR, U = %f\n', ur );
  else
    fprintf ( 1, '  At X = XR, U'' = %f\n', ur );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Number of quadrature points per element is %d\n', nquad );

  return
end
function output ( f, ibc, indx, nsub, nu, ul, ur, xn )

%*****************************************************************************80
%
%% OUTPUT prints out the computed solution at the nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real F(NU), the solution of the linear equations.
%
%    Input, integer IBC.
%    IBC declares what the boundary conditions are.
%    1, at the left endpoint, U has the value UL,
%       at the right endpoint, U' has the value UR.
%    2, at the left endpoint, U' has the value UL,
%       at the right endpoint, U has the value UR.
%    3, at the left endpoint, U has the value UL,
%       and at the right endpoint, U has the value UR.
%    4, at the left endpoint, U' has the value UL,
%       at the right endpoint U' has the value UR.
%
%    Input, integer INDX(1:N+1).
%    For a node I, INDX(I) is the index of the unknown
%    associated with node I.
%    If INDX(I) is equal to -1, then no unknown is associated
%    with the node, because a boundary condition fixing the
%    value of U has been applied at the node instead.
%    Unknowns are numbered beginning with 1.
%    If IBC is 2 or 4, then there is an unknown value of U
%    at node 0, which will be unknown number 1.  Otherwise,
%    unknown number 1 will be associated with node 1.
%    If IBC is 1 or 4, then there is an unknown value of U
%    at node N, which will be unknown N or N+1,
%    depending on whether there was an unknown at node 0.
%
%    integer NSUB.
%    The number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    Input, integer NU.
%    NU is the number of unknowns in the linear system.
%    Depending on the value of IBC, there will be N-1,
%    N, or N+1 unknown values, which are the coefficients
%    of basis functions.
%
%    Input, real UL.
%    If IBC is 1 or 3, UL is the value that U is required
%    to have at X = XL.
%    If IBC is 2 or 4, UL is the value that U' is required
%    to have at X = XL.
%
%    Input, real UR.
%    If IBC is 2 or 3, UR is the value that U is required
%    to have at X = XR.
%    If IBC is 1 or 4, UR is the value that U' is required
%    to have at X = XR.
%
%    Input, real XN(1:N+1).
%    XN(I) is the location of the I-th node.  XN(1) is XL,
%    and XN(N+1) is XR.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'First 10 entries of computed solution:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Node        X(I)          U(I)\n' );
  fprintf ( 1, '\n' );

  for i = 0 : min ( nsub, 9 )

    if ( i == 0 )
      if ( ibc == 1 || ibc == 3 )
        u = ul;
      else
        u = f(indx(i+1));
      end
    elseif ( i == nsub )
      if ( ibc == 2 || ibc == 3 )
        u = ur;
      else
        u = f(indx(i+1));
      end
    else
      u = f(indx(i+1));
    end

    fprintf ( 1, '  %6d  %12f  %12f\n', i, xn(i+1), u );

  end

  return
end
function [ phii, phiix ] = phi ( il, x, xleft, xrite )

%*****************************************************************************80
%
%% PHI evaluates a linear basis function and its derivative.
%
%  Discussion:
%
%    In any interval, there are just two basis functions.  The first
%    basis function is a line which is 1 at the left endpoint
%    and 0 at the right.  The second basis function is 0 at
%    the left endpoint and 1 at the right.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer IL, the index of the basis function.
%    1, the function which is 1 at XLEFT and 0 at XRITE.
%    2, the function which is 0 at XLEFT and 1 at XRITE.
%
%    Input, real X, the evaluation point.
%
%    Input, real XLEFT, XRITE, the left and right
%    endpoints of the interval.
%
%    Output, real PHII, PHIIX, the value of the
%    basis function and its derivative at X.
%
  if ( xleft <= x && x <= xrite )

    if ( il == 1 )
      phii = ( xrite - x ) / ( xrite - xleft );
      phiix = -1.0 / ( xrite - xleft );
    else
      phii = ( x - xleft ) / ( xrite - xleft );
      phiix = 1.0 / ( xrite - xleft );
    end
%
%  If X is outside of the interval, then the basis function
%  is always zero.
%
  else

    phii = 0.0;
    phiix = 0.0;

  end

  return
end
function value = pp ( x )

%*****************************************************************************80
%
%% PP returns the value of the coefficient function P(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of P(X).
%
  value = 1.0;

  return
end
function value = qq ( x )

%*****************************************************************************80
%
%% QQ returns the value of the coefficient function Q(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of Q(X).
%
  value = 0.0;

  return
end
function r8vec_print_some ( n, a, i_lo, i_hi, title )

%*****************************************************************************80
%
%% R8VEC_PRINT_SOME prints "some" of an R8VEC.
%
%  Discussion:
%
%    An R8VEC is a vector of R8 values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 September 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the vector.
%
%    Input, real A(N), the vector to be printed.
%
%    Input, integer MAX_PRINT, the maximum number of lines to print.
%
%    Input, string TITLE, a title.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );
  fprintf ( 1, '\n' );

  for i = max ( 1, i_lo ) : min ( n, i_hi )
    fprintf ( 1, '  %8d: %12f\n', i, a(i) );
  end

  return
end
function u = solve ( adiag, aleft, arite, f, nu )

%*****************************************************************************80
%
%% SOLVE solves a tridiagonal matrix system of the form A*x = b.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real ADIAG(NU), ALEFT(NU), ARITE(NU).
%    the diagonal, left and right entries of the equations.
%    Note that for the first equation, there is no ALEFT
%    coefficient, and for the last, there is no ARITE.
%    So there is no need to store a value in ALEFT(1), nor
%    in ARITE(NU).
%
%    Input, real F(NU),  the right hand side of the linear
%    system to be solved.
%
%    Input, integer NU.
%    NU is the number of equations to be solved.
%
%    Output, real U(NU), the solution of the linear system.
%
  arite(1) = arite(1) / adiag(1);
  for i = 2 : nu-1
    adiag(i) = adiag(i) - aleft(i) * arite(i-1);
    arite(i) = arite(i) / adiag(i);
  end
  adiag(nu) = adiag(nu) - aleft(nu) * arite(nu-1);

  u = zeros ( nu, 1 );

  u(1) = f(1) / adiag(1);
  for i = 2 : nu
    u(i) = ( f(i) - aleft(i) * u(i-1) ) / adiag(i);
  end

  for i = nu-1 : -1 : 1
    u(i) = u(i) - arite(i) * u(i+1);
  end

  return
end
function system_print ( adiag, aleft, arite, f, nu )

%*****************************************************************************80
%
%% SYSTEM_PRINT prints out the tridiagonal linear system to be solved.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ADIAG(NU), ALEFT(NU), ARITE(NU),
%    the diagonal, left and right entries of the equations.
%
%    Input, real F(NU), the right hand side of the linear system.
%
%    Input, integer NU, the number of equations to be solved.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'First 10 rows of tridiagonal linear system:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Equation   ALEFT         ADIAG         ARITE         RHS\n' );
  fprintf ( 1, '\n' );

  for i = 1 : min ( nu, 10 )

    fprintf ( 1, '%3d', i );

    if ( i == 1 )
      fprintf ( 1, '              ' );
    else
      fprintf ( 1, '  %12f', aleft(i) );
    end

    fprintf ( 1, '  %12f', adiag(i) );

    if ( i < nu )
      fprintf ( 1, '  %12f', arite(i) );
    else
      fprintf ( 1, '              ' );
    end

    fprintf ( 1, '  %12f\n', f(i) );

  end

  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 [edges,didSucceed]=graph4VertexDeg(d)
%%GRAPH4VERTEXDEG Given a list of the degrees of vertices in a
%           non-directional graph, get a set of edges representing a graph
%           that satisfies the degree profile, or indicate if no such graph
%           can exist. Note that a set of edges for a degree profile is not
%           unique. This just returns one set.
%
%INPUTS: d An nX1 or 1Xn vector of the degrees of the n vertices. These are
%          integer values >=0. They represent the number of edges touching
%          the nodes.
%
%OUTPUTS: edges A 2XnumEdge set of edges that satisfies the degree profile.
%               An empty matrix is returned if all edges have zero degree
%               or if no set ofvedges exists that can result in the given
%               degree profile. An edge [i;j] goes between vertex i and
%               vertex j and adds to the degree count for both vertices.
%    didSucceed This is true if an edges exists that can satisfy d.
%               Otherwise, this is false and edges is an empty matrix.
%
%This function implements Algorithm H in Chapter 7 of [1].
%
%EXAMPLE:
% [edges,didSucceed]=graph4VertexDeg([2;1;6;3;2;1;1])
%The algorithm in the example will succeed and the edge set is
%edges=[7,6,2,5,5,1,1,4;
%       3,3,3,4,3,4,3,3];
%
%REFERENCES:
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4A: Combinatorial
%    Algorithms, Part I, Boston: Addison-Wesley, 2011.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[d,idx]=sort([d(:);0],'descend');

%Allocate space
maxEdges=ceil(sum(d)/2);
edges=zeros(2,maxEdges);
c=zeros(d(1),1);

numEdges=0;

%Step H1
k=d(1);
j=0;
while(k>0)
    j=j+1;
    while(k>d(j+1))
        c(k)=j;
        k=k-1;
    end
end
%If all of the d's are zero.
if(j==0)
    edges=[];
    didSucceed=true;
    return;
end

while(1)
    %Step H2, find n.
    n=c(1);
    if(n==0)
        %Shrink to fit and restore the original indexation.
        edges=idx(edges(:,1:numEdges));
        %Restore the original indexation.
        didSucceed=true;
        return;
    elseif(d(1)>=n)
        edges=[];
        didSucceed=false;
        return;
    end

    %Step H3
    i=1;
    t=d(1);
    r=c(t);
    j=d(n);

    %Step H4, generate a new edge.
    while(1)
        c(j)=c(j)-1;
        m=c(t);
        numEdges=numEdges+1;
        edges(:,numEdges)=[n;m];
        d(m)=d(m)-1;
        c(t)=m-1;
        j=j-1;
        if(j==0)
            break;
        elseif(m==i)
            i=r+1;
            t=d(i);
            r=c(t);
        end
    end
end

end

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


function mean = half_normal_mean ( a, b )

%*****************************************************************************80
%
%% HALF_NORMAL_MEAN returns the mean of the Half Normal PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < B.
%
%    Output, real MEAN, the mean of the PDF.
%
  mean = a + b * sqrt ( 2.0 / pi );

  return
end


function [d2f, err] = tapas_riddersdiff2(f, x, varargin)
% Differentiates the function f *twice* at point x according to Ridders' method:
%
% Ridders, CJF. (1982). Accurate computation of F'(x) and F'(x) F''(x). Advances in Engineering
%     Software, 4(2), 75-6.
%
% INPUT:
%    f             Function handle of a scalar real function of one real variable
%    x             Point at which to differentiate f
%
% OUTPUT:
%    d2f           Second derivative of f at x
%    err           Error estimate
%
% OPTIONS:
%    Optionally, the third argument of the function can be a structure containing further
%    settings for Ridder's method.
%
%    varargin{1}.init_h      Initial finite difference (default: 1)
%    varargin{1}.div         Divisor used to reduce h on each step (default: 1.2)
%    varargin{1}.min_steps   Minimum number of steps in h (default: 3)
%    varargin{1}.max_steps   Maximum number of steps in h (default: 100)
%    varargin{1}.tf          Terminate if last step worse than preceding by a factor of tf
%                            (default: 2)
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is released under the terms of the GNU General Public Licence (GPL), version 3. You can
% redistribute it and/or modify it under the terms of the GPL (either version 3 or, at your option,
% any later version). For further details, see the file COPYING or <http://www.gnu.org/licenses/>.

    % Defaults
    init_h     = 1;
    div        = 1.2;
    min_steps  = 3;
    max_steps  = 100;
    tf         = 2;
    d2f        = NaN;
    err        = realmax;

    % Overrides
    if nargin > 2
        options = varargin{1};

        if isfield(options,'init_h')
            init_h = options.init_h;
        end

        if isfield(options,'div')
            div = options.div;
        end

        if isfield(options,'min_steps')
            min_steps = options.min_steps;
        end

        if isfield(options,'max_steps')
            max_steps = options.max_steps;
        end

        if isfield(options,'tf')
            tf = options.tf;
        end
    end

    % Initialize matrix of polynomial interpolation values
    P = NaN(max_steps);

    % Initialize finite difference step
    h = init_h;

    % Approximate 2nd derivative at initial step
    P(1,1) = (f(x+h)-2*f(x)+f(x-h))/h^2;

    % Loop through rows of P (i.e., steps of h)
    for i = 2:max_steps

        % New step size
        h = h/div;

        % Approximate 2nd derivative at this step
        P(i,1) = (f(x+h)-2*f(x)+f(x-h))/h^2;

        % Use square of div for extrapolation because errors increase
        % quadratically with h (here, of course, they decrease quadratically
        % because we're reducing h...)
        divsq = div^2;
        t = divsq;

        % Fill the current row using Richardson extrapolation
        for j = 2:i

            % Richardson
            P(i,j) = (t*P(i,j-1)-P(i-1,j-1))/(t-1);

            % Increment extrapolation factor
            t = t*divsq;

            % Error on this trial is defined as the maximum absolute difference
            % to the extrapolation parents
            currerr = max(abs(P(i,j)-P(i,j-1)),abs(P(i,j)-P(i-1,j-1)));

            if currerr < err
                err = currerr;
                d2f = P(i,j);
            end
        end

        % Stop if errors start increasing (to be expected for very small
        % values of h)
        if i > min_steps && abs(P(i,i)-P(i-1,i-1)) > tf*err
            return
        end
    end
end


function [t,X] = smdplot(t,X)
% See Article 2.6
% [t,X]=smdplot(t,X)
% This function plots the response and animates the
% motion of a damped linear harmonic oscillator
% characterized by the differential equation
% m*x''+c*x'+k*x=f1*cos(w*t)+f2*sin(w*t)
% with initial conditions x(0)=x0, x'(0)=v0.
% The animation depicts forced motion of a block
% attached to a wall by a spring. The block
% slides on a horizontal plane which provides
% viscous damping.

% Default data case used when no input arguments
% are given
if nargin==0
  m=1; c=.3; k=1; f1=1.5; f2=0; w=2; x0=0; v0=2;
  %tmax=30; nt=250; t=linspace(0,tmax,nt);
  tmax=25; nt=200; t=linspace(0,tmax,nt);
  X=smdsolve(m,c,k,f1,f2,w,x0,v0,t);
end

% Plot the displacement versus time
plot(t,X), xlabel('time'), ylabel('displacement')
title(...
'FORCED RESPONSE OF A DAMPED HARMONIC OSCILLATOR')
grid on, shg, pause(3)

% Add a block and a spring to the displacement
xmx=max(abs(X)); X=X/1.1/xmx;
xb=[0,0,1,1,0,0]/2; yb=[0,-1,-1,1,1,0]/2;

% Make an arrow tip
d=.08; h=.05;
xtip=[0,-d,-d,0]; ytip=[0,0,0,h,-h,0];

% Add a spring and a block to the response
[xs,ys]=spring; nm=length(X); ns=length(xs);
nb=length(xb); x=zeros(nm,ns+nb);y=[ys,yb];
for j=1:nm, x(j,:)=[-1+(1+X(j))*xs,X(j)+xb];end
r=[min(x(:)),max(x(:))+1,-2,2];
rx=r([1 1 2]); ry=[.5,-.5,-.5]; close;

% Plot the motion
for j=1:nm
   % Compute and scale the applied force
   f=f1*cos(w*t(j))+f2*sin(w*t(j));
   f=.3*f; fa=abs(f); sf=sign(f);
   xj=x(j,:); xmaxj=max(xj);
   if sf>0
      xforc=xmaxj+[0,fa,fa+xtip];
   else
      xforc=xmaxj+[fa,0,-xtip];
   end

   % Plot the spring, block, and force
   plot(xj,y,rx,ry,'k',xforc,ytip,'r')
   title('FORCED MOTION WITH DAMPING')
   axis(r), axis('off'), drawnow
   figure(gcf), pause(.05)
end

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

function [x,y] = spring(len,ht)
% This function generates a set of points
% defining a spring

if nargin==0, len=1; ht=.125; end
x=[0,.5,linspace(1,11,10),11.5,12];
y=[ones(1,5);-ones(1,5)];
y=[0;0;y(:);0;0]'; y=ht/2/max(y)*y;
x=len/max(x)*x;

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

function [x,v]=smdsolve(m,c,k,f1,f2,w,x0,v0,t)
% [x,v]=smdsolve(m,c,k,f1,f2,w,x0,v0,t)
% This function solves the differential equation
% m*x''(t)+c*x'(t)+k*x(t)=f1*cos(w*t)+f2*sin(w*t)
% with x(0)=x0 and x'(0)=v0
%
% m,c,k  - mass, damping and stiffness coefficients
% f1,f2  - magnitudes of cosine and sine terms in
%          the forcing function
% w      - frequency of the forcing function
% t      - vector of times to evaluate the solution
% x,v    - computed position and velocity vectors

ccrit=2*sqrt(m*k); wn=sqrt(k/m);

% If the system is undamped and resonance will
% occur, add a little damping
if c==0 & w==wn; c=ccrit/1e6; end;

% If damping is critical, modify the damping
% very slightly to avoid repeated roots
zeta=c/ccrit; if zeta==1, zeta=zeta+1e-6; end

% Forced response solution
a=(f1-i*f2)/(k-m*w^2+i*c*w);
X0=real(a); V0=real(i*w*a);
X=real(a*exp(i*w*t)); V=real(i*w*a*exp(i*w*t));

% Homogeneous solution
r=sqrt(zeta^2-1); s1=wn*(-zeta+r); s2=wn*(-zeta-r);
p=[1,1;s1,s2]\[x0-X0;v0-V0];

% Total solution satisfying the initial conditions
x=X+real(p(1)*exp(s1*t)+p(2)*exp(s2*t));
v=V+real(p(1)*s1*exp(s1*t)+p(2)*s2*exp(s2*t));


function c = t_project_coefficients ( n, f )

%*****************************************************************************80
%
%% T_PROJECT_COEFFICIENTS: function projected onto T(0:n,x).
%
%  Discussion:
%
%    It is assumed that the interval of definition is -1 <= x <= +1.
%
%    Over this interval, f(x) will be well approximated by
%
%      f(x) approx sum ( 0 <= i <= n ) c(i) * T(i,x)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the highest order polynomial to compute.
%
%    Input, function handle F, of the form
%      function v = f ( x )
%
%    Output, real C(N+1,1), the projection coefficients of f(x) onto
%    T(0,x) through T(n,x).
%
  for k = 1 : n + 1
    y = cos ( pi * ( k - 0.5 ) / ( n + 1 ) );
    d(k) = f ( y );
  end

  fac = 2.0 / ( n + 1 );
  for j = 1 : ( n + 1 )
    sum = 0.0;
    for k = 1 : ( n + 1 )
      sum = sum + d(k) * cos ( ( pi * ( j - 1 ) ) * ( ( k - 0.5 ) / ( n + 1 ) ) );
    end
    c(j) = fac * sum;
  end

  c(1) = c(1) / 2.0;

  return
end


function [ok,tp,tq] = linenear(pa,pb,pc,pd)
%LINENEAR calc. the nearest points on line segments embedded
%in d-dimensions. Line paramters are bounded on [-1,+1].

%-----------------------------------------------------------
%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 10/10/2017
%-----------------------------------------------------------

    m1 = (pa+pb) * +.5 ;
    D1 = (pb-pa) * +.5 ;

    m2 = (pc+pd) * +.5 ;
    D2 = (pd-pc) * +.5 ;

    r1 = sum(m2.*D1,2) ...
       - sum(m1.*D1,2) ;
    r2 = sum(m1.*D2,2) ...
       - sum(m2.*D2,2) ;

    A1 = sum(D1.*D1,2) ;
    A2 =-sum(D1.*D2,2) ;
    A3 =-sum(D1.*D2,2) ;
    A4 = sum(D2.*D2,2) ;

    dd = A1.*A4 - A2.*A3 ;

    tp = A4.*r1 - A2.*r2 ;
    tq =-A3.*r1 + A1.*r2 ;

    rt = max(abs([A1,A2,A3,A4]),[],2);
    rt = rt * eps ^ .8 ;

    ok = abs(dd) > +rt ;

    tp(~ok) = +0. ;
    tq(~ok) = +0. ;

    tp(ok) = tp(ok) ./ dd(ok) ;
    tq(ok) = tq(ok) ./ dd(ok) ;

end


% generating channels with fliter method and spectrum method
% evaluate autocorrelation functions
%%
clear all
clc
close all

% doppler
fDTs=0.01;
fD=100;
% sampling time
Ts=fDTs/fD;

Ns=1E5;

%% spectrum method
h1=flat_spec(Ns,fD,fDTs);

% autocorrelation
Acn=xcorr(h1,'biased');
l=length(Acn);
% truncate
w=ceil(4/fDTs);
TAc=Acn(ceil(l/2)-w:ceil(l/2)+w);

lt=length(TAc);
t=-Ts*floor(lt/2):Ts:Ts*floor(lt/2);
% theoritical value of autocorrelation
Ac_th=besselj(0,2*pi*fD*t);

figure;
plot(t,real(Ac_th),t,real(TAc),'r');
title('Autocorrelation');
xlabel('time');
legend('theory','spec. method');
grid on

%% filter method
h1=flat_filter(Ns,fD,fDTs);

Acn=xcorr(h1,'biased');
l=length(Acn);
w=ceil(4/fDTs);
TAc=Acn(ceil(l/2)-w:ceil(l/2)+w);

l=length(TAc);
t=-Ts*floor(l/2):Ts:Ts*floor(l/2);
% theory
Ac_th=besselj(0,2*pi*fD*t);

figure;
plot(t,real(Ac_th),t,real(TAc),'r');
title('Autocorrelation');
xlabel('time');
legend('theory','filter method');
grid on


function [ pts ] = genRansacTestPoints( ptNum,outlrRatio,inlrStd,inlrCoef )
%GENRANSACTESTPOINTS Generate the points used by RANSAC function
%   PTS = GENRANSACTESTPOINTS(PTNUM,OUTLRRATIO,INLRSTD,INLRCOEF) PTS is
%   2*PTNUM, including PTNUM points, among which ROUND(OUTLRRATIO*PTNUM)
%   are outliers, others are inliers.
%		The inliers are around the line: y = INLRCOEF(1)*x + INLRCOEF(2),
%   INLRSTD is the standard deviation of, the dist between inliers and the
%	line. The outliers

outlrNum = round(outlrRatio*ptNum);
inlrNum = ptNum-outlrNum;

k = inlrCoef(1);
b = inlrCoef(2);
X = (rand(1,inlrNum)-.5)*ptNum; % X is in [-ptNum/2,ptNum/2]
Y = k*X+b;

% add noise for inliers
dist = randn(1,inlrNum)*inlrStd;
theta = atan(k);
X = X+dist*(-sin(theta));
Y = Y+dist*cos(theta);
inlrs = [X;Y];

outlrs = (rand(2,outlrNum)-.5)*ptNum;
% outlrs = (rand(2,outlrNum)-[ones(1,outlrNum)*.5;ones(1,outlrNum)*.1])*ptNum;
pts = [inlrs,outlrs];

end


function p = predictOneVsAll(all_theta, X)
%PREDICT Predict the label for a trained one-vs-all classifier. The labels
%are in the range 1..K, where K = size(all_theta, 1).
%  p = PREDICTONEVSALL(all_theta, X) will return a vector of predictions
%  for each example in the matrix X. Note that X contains the examples in
%  rows. all_theta is a matrix where the i-th row is a trained logistic
%  regression theta vector for the i-th class. You should set p to a vector
%  of values from 1..K (e.g., p = [1; 3; 1; 2] predicts classes 1, 3, 1, 2
%  for 4 examples)

m = size(X, 1);
num_labels = size(all_theta, 1);

% You need to return the following variables correctly
p = zeros(size(X, 1), 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters (one-vs-all).
%               You should set p to a vector of predictions (from 1 to
%               num_labels).
%
% Hint: This code can be done all vectorized using the max function.
%       In particular, the max function can also return the index of the
%       max element, for more information see 'help max'. If your examples
%       are in rows, then, you can use max(A, [], 2) to obtain the max
%       for each row.
%

sig = sigmoid(X * all_theta');
[maxSig, maxSig_2] = max(sig');
p = maxSig_2';

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

end