function f = bohach1_xy ( x, y )

%*****************************************************************************80
%
%% BOHACH1_XY evaluates the Bohachevsky function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Zbigniew Michalewicz,
%    Genetic Algorithms + Data Structures = Evolution Programs,
%    Third Edition,
%    Springer Verlag, 1996,
%    ISBN: 3-540-60676-9,
%    LC: QA76.618.M53.
%
%  Parameters:
%
%    Input, real X, Y, the argument of the function.
%
%    Output, real F, the value of the function at X.
%
  f =       x * x - 0.3 * cos ( 3.0 * pi * x ) ...
    + 2.0 * y * y - 0.4 * cos ( 4.0 * pi * y ) ...
    + 0.7;

  return
end


% Compute the average degree of a node in a graph, defined as
% 2 times the number of edges divided by the number of nodes
%          (every edge is counted towards the degrees twice).
%
% Inputs: adjacency matrix, nxn
% Outputs: float, the average degree, a number between 0 and max(sum(adj))
%
% Note: The average degree is related to the link density, namely:
%       link_density = ave_degree/(n-1), where n is the number of nodes
%
% Other routines used: numNodes.m, numEdges.m
% GB: last update, September 20, 2012

function k=averageDegree(adj)

k=2*numEdges(adj)/numNodes(adj);


%MAIN_doublePendulum.m
%
% This script runs a simulation of a double pendulum

% Use: EoM_Double_Pendulum to write the equations of motion

clear; clc;

%Physical parameters:
P.m1 = 4.0;  % (kg) pendulum mass, link 1
P.m2 = 1.0;  % (kg) pendulum mass, link 2
P.g = 9.81; % (m/s^2) gravity
P.l1 = 1.0; % (m) link 1 length
P.l2 = 2.0; % (m) link 2 length
P.d1 = 0.6*P.l1; % Distance from parent joint to link CoM
P.d2 = 0.4*P.l2; % Distance from parent joint to link CoM
P.I1 = P.m1*P.l1^2/12; %Link 1, moment of inertia
P.I2 = P.m2*P.l2^2/12; %Link 2, moment of inertia

tSpan = [0,10]; %Simulation time interval

z0 = zeros(4,1);
z0(1) = (pi/180)*(-90);  % Link 1, initial abs angle
z0(2) = 0;  % Link 1, initial abs angle rate
z0(3) = (pi/180)*(-90);  % Link 1, initial abs angle
z0(4) = 0;  % Link 1, initial abs angle rate

u = [0;0];

userFunc = @(t,z)doublePendulumDynamics(t,z,u,P);

options = odeset(...
    'AbsTol',1e-6,...
    'RelTol',1e-6,...
    'Vectorized','on');

% Run the simulation!
sol = ode45(userFunc,tSpan,z0,options);

% Break apart solution for plotting
nPlot = 1000;
time = linspace(tSpan(1),tSpan(2),nPlot);
z = deval(sol,time); %Evaluate solution from ode45 at points in time
th1 = z(1,:);
w1 = z(2,:);
th2 = z(3,:);
w2 = z(4,:);
[energy, kinetic, potential] = doublePendulumEnergy(z,P);

% Plotting!

figure(2222);clf;

subplot(3,1,1); hold on;
plot(time,th1,'r-','LineWidth',2);
plot(time,th2,'b-','LineWidth',2);
xlabel('time (s)')
ylabel('angle (rad)');

subplot(3,1,2); hold on;
plot(time,w1,'r-','LineWidth',2);
plot(time,w2,'b-','LineWidth',2);
xlabel('time (s)')
ylabel('angle rate (rad/s)');

subplot(3,1,3);  hold on;
datum = min(potential);
plot(time,energy - datum,'k-','LineWidth',3);
plot(time,kinetic,'LineWidth',2,'color',[0.5,0.1,0.6]);
plot(time,potential - datum,'LineWidth',2,'color',[0.3,0.5,0.2]);
xlabel('time (s)')
ylabel('energy (J)');
legend('total','kinetic','potential');

% Animation
figure(3333); clf;
doublePendulumAnimate(sol,P);


function X = matrandnorm(varargin)
%MATRANDNORM Normalizes columns of X so that each is unit 2-norm.
%
%   X = MATRANDNORM(M,N) creates a random M x N matrix with randomly using
%   normally distributed enries and then rescales the columsn so that each
%   has a unit 2-norm.
%
%   X = MATRANDNORM(X) rescales the columns of X so that each
%   column has a unit 2-norm.
%
%   Examples
%      X = MATRANDNORM(rand(5,5));
%      X = MATRANDNORM(3,2);
%      X = MATRANDNORM(ones(4));
%
%   See also MATRANDORTH, MATRANDNORM, CREATE_PROBLEM, CREATE_GUESS.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

if nargin == 2
    X = randn(varargin{1}, varargin{2});
else
    X = varargin{1};
end

norms = sqrt(sum(X.^2,1));
X = bsxfun(@rdivide,X,norms);


function result = circle_annulus ( func, xc, yc, radius1, radius2, nr )

%*****************************************************************************80
%
%% CIRCLE_ANNULUS approximates an integral in a circular annulus.
%
%  Integration region:
%
%    Points (X,Y) such that
%
%      RADIUS1**2 <= ( X - XC )**2 + ( Y - YC )**2 <= RADIUS2**2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    William Peirce,
%    Numerical Integration Over the Planar Annulus,
%    Journal of the Society for Industrial and Applied Mathematics,
%    Volume 5, Issue 2, June 1957, pages 66-73.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied function of two
%    variables which is to be integrated, of the form:
%      function value = func ( x, y )
%
%    Input, real XC, YC, the center of the circle.
%
%    Input, real RADIUS1, RADIUS2, the radii of the circles.
%
%    Input, integer NR, the order of the rule.  This quantity specifies
%    the number of distinct radii to use.  The number of angles used will
%    be 4*NR, for a total of 4*NR**2 points.
%
%    Output, real RESULT, the approximation to the integral.
%

%
%  Choose radial abscissas and weights.
%
  [ ra, rw ] = legendre_set ( nr );
  a = -1.0E+00;
  b = +1.0E+00;
  c = radius1^2;
  d = radius2^2;
  [ ra, rw ] = rule_adjust ( a, b, c, d, nr, ra, rw );
  ra(1:nr) = sqrt ( ra(1:nr) );
  rw(1:nr) = rw(1:nr) / ( radius2^2 - radius1^2 );
%
%  Set angular abscissas and weights.
%
  nt = 4 * nr;

  tw = 1.0E+00 / nt;
%
%  Approximate the integral.
%
  quad = 0.0E+00;
  for i = 1 : nt
    t = 2.0E+00 * pi * ( i - 1 ) / nt;
    for j = 1 : nr
      x = xc + ra(j) * cos ( t );
      y = yc + ra(j) * sin ( t );
      quad = quad + tw * rw(j) * feval ( func, x, y );
    end
  end

  area = circle_annulus_area_2d ( radius1, radius2 );
  result = quad * area;

  return
end


function lij = fem_basis_1d ( i, j, x )

%*****************************************************************************80
%
%% FEM_BASIS_1D evaluates an arbitrary 1D basis function.
%
%  Discussion:
%
%    Given the maximum degree D for the polynomial basis defined
%    on a reference interval, we have D + 1 monomials
%    of degree at most D.  In each barycentric coordinate, we define
%    D+1 points, so that 0 <= I, J <= D and I+J = D, with
%    (I,J) corresponding to
%    * the basis point X(I,J) = ( I/D );
%    * the basis monomial P(I,J)(X) = X^I.
%
%    For example, with D = 2, we have simply:
%
%      A---B---C
%
%    with
%
%       I J    X      P(I,J)(X)
%
%    A (0 2) ( 0.0 )  1
%    B (1 1) ( 0.5 )  x
%    C (2 0) ( 1.0 )  x^2
%
%    Now instead of the monomials P(I,J)(X), we want a set of
%    polynomials L(I,J)(X) which span the same space, but have
%    the Lagrange property, namely L(I,J) (X) is 1 if X is
%    equal to X(I,J), and 0 if X is equal to any other
%    of the basis points.
%
%    This is easily arranged.  Given an index (I,J), we compute
%    1) I factors of the form (X-0)   * (X-1/D)   * ... * (X-(I-1)/D);
%    2) J factors of the form ???
%
%    This results in the product of I+J linear factors, in other words,
%    a polynomial of degree D.  This polynomial is 0 at all basis points
%    except X(I,J).  If we divide this polynomial by its value at
%    the basis point, we arrive at the desired Lagrange polynomial
%    L(I,J)(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 October 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, J, the integer barycentric coordinates of the basis
%    function, 0 <= I, J.  The polynomial degree D = I + J
%
%    Input, real X, the evaluation point.
%
%    Output, real LIJ, the value of the basis function at X.
%
  d = i + j;
  lij = 1.0;
  cij = 1.0;
  for p = 0 : i - 1
    lij = lij * ( d * x - p );
    cij = cij * (     i - p );
  end
  for p = 0 : j - 1
    lij = lij * ( d * x - ( d - p ) );
    cij = cij * (     i - ( d - p ) );
  end

  lij = lij / cij;

  return
end


function plane2 = normalizePlane(plane1)
%NORMALIZEPLANE Normalize parametric representation of a plane
%
%   PLANE2 = normalizePlane(PLANE1);
%   Transforms the plane PLANE1 in the following format:
%   [X0 Y0 Z0  DX1 DY1 DZ1  DX2 DY2 DZ2], where:
%   - (X0, Y0, Z0) is a point belonging to the plane
%   - (DX1, DY1, DZ1) is a first direction vector
%   - (DX2, DY2, DZ2) is a second direction vector
%   into another plane, with the same format, but with:
%   - (x0 y0 z0) is the closest point of plane to the origin
%   - (DX1 DY1 DZ1) has norm equal to 1
%   - (DX2 DY2 DZ2) has norm equal to 1 and is orthogonal to (DX1 DY1 DZ1)
%
%   See also:
%   planes3d, createPlane
%
%   ---------
%   author: David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 21/02/2005.
%

%   HISTORY
%   21/08/2009 compute origin after computation of vectors (more precise)
%       and add support for several planes.

% compute first direction vector
d1  = normalizeVector3d(plane1(:,4:6));

% compute second direction vector
n   = normalizeVector3d(planeNormal(plane1));
d2  = -normalizeVector3d(crossProduct3d(d1, n));

% compute origin point of the plane
origins = repmat([0 0 0], [size(plane1, 1) 1]);
p0 = projPointOnPlane(origins, [plane1(:,1:3) d1 d2]);

% create the resulting plane
plane2 = [p0 d1 d2];


function [J, grad] = cofiCostFunc(params, Y, R, num_users, num_movies, ...
                                  num_features, lambda)
%COFICOSTFUNC Collaborative filtering cost function
%   [J, grad] = COFICOSTFUNC(params, Y, R, num_users, num_movies, ...
%   num_features, lambda) returns the cost and gradient for the
%   collaborative filtering problem.
%

% Unfold the U and W matrices from params
X = reshape(params(1:num_movies*num_features), num_movies, num_features);
Theta = reshape(params(num_movies*num_features+1:end), ...
                num_users, num_features);

% You need to return the following values correctly
J = 0;
X_grad = zeros(size(X));
Theta_grad = zeros(size(Theta));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost function and gradient for collaborative
%               filtering. Concretely, you should first implement the cost
%               function (without regularization) and make sure it is
%               matches our costs. After that, you should implement the
%               gradient and use the checkCostFunction routine to check
%               that the gradient is correct. Finally, you should implement
%               regularization.
%
% Notes: X - num_movies  x num_features matrix of movie features
%        Theta - num_users  x num_features matrix of user features
%        Y - num_movies x num_users matrix of user ratings of movies
%        R - num_movies x num_users matrix, where R(i, j) = 1 if the
%            i-th movie was rated by the j-th user
%
% You should set the following variables correctly:
%
%        X_grad - num_movies x num_features matrix, containing the
%                 partial derivatives w.r.t. to each element of X
%        Theta_grad - num_users x num_features matrix, containing the
%                     partial derivatives w.r.t. to each element of Theta
%

temp = (X*Theta').*R;
J = sum( sum( (temp - Y.*R).^2) )/2.0 + (lambda/2) * ( sum(sum( X.^2 )) + sum(sum( Theta.^2 )) );
% J = sum( sum( (temp - Y.*R).^2) )/2.0 + (lambda/2) * ( sum(sum( X.^2 )) + sum(sum( Theta.^2 )) ) ;

X_grad = (temp - Y.*R) * Theta + lambda * X;
Theta_grad = (temp - Y.*R)' * X + lambda * Theta;

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

grad = [X_grad(:); Theta_grad(:)];

end


function geometry_test013 ( )

%*****************************************************************************80
%
%% TEST013 tests CIRCLE_LUNE_CENTROID_2D, CIRCLE_SECTOR_CENTROID_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  n_test = 12;

  r = 2.0;
  center(1:2,1) = [ 5.0; 3.0 ];
  theta1 = 0.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST013\n' );
  fprintf ( 1, '  CIRCLE_LUNE_CENTROID_2D computes the centroid of a\n' );
  fprintf ( 1, '    circular lune, defined by joining the endpoints\n' );
  fprintf ( 1, '    of a circular arc.\n' );
  fprintf ( 1, '  CIRCLE_SECTOR_CENTROID_2D computes the centroid of a\n' );
  fprintf ( 1, '    circular sector, defined by joining the endpoints\n' );
  fprintf ( 1, '    of a circular arc to the center.\n' );

  circle_imp_print_2d ( r, center, '  The implicit circle:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The first angle of our lune and sector is always 0.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '                         Lune                       Sector\n' );
  fprintf ( 1, '  THETA2           X             Y             X             Y\n' );
  fprintf ( 1, '\n' );

  for i = 0 : n_test

    theta2 = i * 2.0 * pi / n_test;

    centroid1(1:2,1) = circle_lune_centroid_2d ( r, center, theta1, theta2 );

    centroid2(1:2,1) = circle_sector_centroid_2d ( r, center, theta1, theta2 );

    fprintf ( 1, '  %12f  %12f  %12f  %12f  %12f\n', ...
      theta2, centroid1(1:2,1), centroid2(1:2,1) );

  end

  return
end


% StackExchange Mathematics Q3872982
% https://math.stackexchange.com/questions/3872982
% Solve Linear Least Squares with Squared L2 Norm Regularization (Tikhonov
% / Ridge Regression) with Non Negativity Constraint Using FISTA
% References:
%   1.
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     26/12/2020
%   *   First release.

%% General Parameters

subStreamNumberDefault = 0;2165;42; %<! Set to 0 for Random

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Parameters

numRows         = 100;
numCols         = 20;
paramLambda     = 0.1;

% Solvers parameters
numIterations       = 5000;
stepSizeGd          = 5e-5;
stepSizeMomentum    = 0.8;
stepSizeAccel       = 0.4;

%% Load / Generate Data

mA = randn(numRows, numCols);
vB = randn(numRows, 1);

hObjFun = @(vX) 0.5 * sum((mA * vX - vB) .^ 2) + (0.5 * paramLambda * sum(vX .^ 2));

mAA = mA.' * mA;
vAb = mA.' * vB;

hG = @(vX) (mAA * vX) - vAb + (paramLambda * vX); %<! Gradient of the Objection Function
hP = @(vX) max(vX, 0); %<! Projection onto the constraint set

solverIdx       = 0;
cMethodString   = {};

mObjFunValMse   = zeros([numIterations, 1]);
mSolMse         = zeros([numIterations, 1]);

%% Solution by CVX

solverString = 'CVX';

% cvx_solver('SDPT3'); %<! Default, Keep numRows low
% cvx_solver('SeDuMi');
% cvx_solver('Mosek'); %<! Can handle numRows > 500, Very Good!
% cvx_solver('Gurobi');

hRunTime = tic();

cvx_begin('quiet')
% cvx_begin()
    % cvx_precision('best');
    variable vX(numCols, 1);
    minimize( 0.5 * sum_square(mA * vX - vB) + (0.5 * paramLambda * square_pos(norm(vX))) );
    subject to
        vX >= 0;
cvx_end

runTime = toc(hRunTime);

% vX = mX(:);

disp([' ']);
disp([solverString, ' Solution Summary']);
disp(['The ', solverString, ' Solver Status - ', cvx_status]);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

sCvxSol.vXCvx     = vX;
sCvxSol.cvxOptVal = hObjFun(vX);

%% Solution by Projected Gradient Descent

solverIdx                   = solverIdx + 1;
cLegendString{solverIdx}    = ['Projected Gradient Method'];

hRunTime = tic();

[vX, mX] = ProjectedGd(zeros(numCols, 1), hG, hP, numIterations, stepSizeGd);

runTime = toc(hRunTime);

disp([' ']);
disp([cLegendString{solverIdx}, ' Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

[mObjFunValMse, mSolMse] = UpdateAnalysisData(mObjFunValMse, mSolMse, mX, hObjFun, sCvxSol, solverIdx);

%% Solution by Projected Gradient Descent with Momentum

solverIdx                   = solverIdx + 1;
cLegendString{solverIdx}    = ['Projected Gradient Descent with Momentum'];

hRunTime = tic();

[vX, ~, mX] = ProjectedGdMomentum(zeros(numCols, 1), zeros(numCols, 1), hG, hP, numIterations, stepSizeGd, stepSizeMomentum);

runTime = toc(hRunTime);

disp([' ']);
disp([cLegendString{solverIdx}, ' Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

[mObjFunValMse, mSolMse] = UpdateAnalysisData(mObjFunValMse, mSolMse, mX, hObjFun, sCvxSol, solverIdx);

%% Solution by Projected Gradient Descent with Nesterov Acceleration

solverIdx                   = solverIdx + 1;
cLegendString{solverIdx}    = ['Projected Gradient Descent with Nesterov Acceleration'];

hRunTime = tic();

[vX, ~, mX] = ProjectedGdAccel(zeros(numCols, 1), zeros(numCols, 1), hG, hP, numIterations, stepSizeGd, stepSizeAccel);

runTime = toc(hRunTime);

disp([' ']);
disp([cLegendString{solverIdx}, ' Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

[mObjFunValMse, mSolMse] = UpdateAnalysisData(mObjFunValMse, mSolMse, mX, hObjFun, sCvxSol, solverIdx);

%% Solution by Projected Gradient Descent with FISTA

solverIdx                   = solverIdx + 1;
cLegendString{solverIdx}    = ['Projected Gradient Descent with FISTA Acceleration'];

hRunTime = tic();

[vX, ~, mX] = ProjectedGdFista(zeros(numCols, 1), zeros(numCols, 1), hG, hP, numIterations, stepSizeGd);
% [vX, mX] = SolveLsFista(zeros(numCols, 1), mA, vB, paramLambda, numIterations, stepSizeGd);

runTime = toc(hRunTime);

disp([' ']);
disp([cLegendString{solverIdx}, ' Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

[mObjFunValMse, mSolMse] = UpdateAnalysisData(mObjFunValMse, mSolMse, mX, hObjFun, sCvxSol, solverIdx);

%% Display Results

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);

hAxes       = subplot(2, 1, 1);
hLineSeries = plot(1:numIterations, 10 * log10(mObjFunValMse));
set(hLineSeries, 'LineWidth', lineWidthNormal);
set(get(hAxes, 'Title'), 'String', ['Objective Function Value vs. Optimal Value (CVX)'], ...
    'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', 'Iteration Number', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', '$ 10 \log_{10} {\left( \left| f \left( x \right) - f \left( {x}_{CVX} \right) \right| \right)}^{2} $', ...
    'FontSize', fontSizeAxis, 'Interpreter', 'latex');
set(hAxes, 'XLim', [1, numIterations]);
hLegend = ClickableLegend(cLegendString);

hAxes       = subplot(2, 1, 2);
hLineSeries = plot(1:numIterations, 10 * log10(mSolMse));
set(hLineSeries, 'LineWidth', lineWidthNormal);
set(get(hAxes, 'Title'), 'String', ['Solution Error Norm'], ...
    'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', 'Iteration Number', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', '$ 10 \log_{10} \left( {\left\| x - {x}_{CVX} \right\|}_{2}^{2} \right) $', ...
    'FontSize', fontSizeAxis, 'Interpreter', 'latex');
set(hAxes, 'XLim', [1, numIterations]);
hLegend = ClickableLegend(cLegendString);

if(generateFigures == ON)
    % saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
    print(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png'], '-dpng', '-r0'); %<! Saves as Screen Resolution
end

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function [labels, distances] = elec_distance(elec_labels,X,Y,Z,xo,yo,zo)

% elec_distance - Calculates spherical interelectrode distances (arc length).
%
% Usage: [labels, distances] = elec_distance(elec_labels,X,Y,Z,xo,yo,zo)
%
% Notes:    Arc length method assumes a centroid at (xo,yo,zo) = (0,0,0)
%           and input values are in rectangular Cartesian coordinates.
%
%           Sphere radius is estimated by the average radius from
%           (xo,yo,zo) to any 2 pairs of electrodes.  It will vary
%           from one pair to another, but this method works well for
%           small theta (eg, nearest neighbours).
%
%           Returns 2 matrices, one for paired electrode labels and
%           another for spherical arc length estimates.
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:54 $

% Author:   Darren.Weber_at_radiology.ucsf.edu
% Created:  18/05/00 - linear distance
% Modified: 24/06/01 - spherical arc length (should be OK for small theta
%                      but for large theta, elliptical arc length may be
%                      preferable).
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % initialise centroid, unless input parameters defined
    if ~exist('xo','var')  xo = 0;   end
    if ~exist('yo','var')  yo = 0;   end
    if ~exist('zo','var')  zo = 0;   end

    fprintf('%s\n', 'Calculating inter-electrode spherical arc length.');

    labels = cell(1);
    distances = [];

    % convert numerical electrode labels to cell strings
    if isnumeric(elec_labels) > 0
        if size(elec_labels,1) > size(elec_labels,2)
            elec_labels = cellstr(strcat(num2str(elec_labels)));
        else
            elec_labels = cellstr(strcat(num2str(elec_labels')));
        end
    end

    A = [ (X-xo) (Y-yo) (Z-zo) ];  B = A;              % define electrode vectors A,B, given centroid (xo,yo,zo)
    rows = 50;                                         % progress indicator
    for a =   1:length(X)
        fprintf('.'); if( a == rows ) fprintf('\n'); rows = rows + 50; end  % progress indicator

        Aa = A(a,:);
        A_len = sqrt( sum( Aa.^2 ) );                        % length of electrode vector A

        for b = 1:length(X)

            Bb = B(b,:);
            B_len = sqrt ( sum(Bb.^2) );                     % length of electrode vector B

            if( Aa == Bb )
                arc_len = 0;                                    % no distance from electrode to itself
            else
                r = (A_len + B_len)/2;                          % estimate sphere radius from A_len and B_len
                theta = acos( dot(Aa,Bb) / (A_len * B_len) );   % Angle between A & B, in radians
                arc_len = r * theta;                            % arc length = radius * theta
            end

            distances(a, b) = arc_len;
            labels(a, b) = strcat(elec_labels(a), ', ', elec_labels(b));
        end
    end
    fprintf('\n');


function breadth = polyhedronMeanBreadth(vertices, edges, faces)
%POLYHEDRONMEANBREADTH Mean breadth of a convex polyhedron
%
%   BREADTH = polyhedronMeanBreadth(V, E, F)
%   Return the mean breadth (average of polyhedron caliper diameter over
%   all direction) of a convex polyhedron.
%
%   The mean breadth is computed using the sum, over the edges of the
%   polyhedron, of the edge dihedral angles multiplied by the edge length,
%   the final sum being divided by (4*PI).
%
%   Note: the function assumes that the faces are correctly oriented. The
%   face vertices should be indexed counter-clockwise when considering the
%   supporting plane of the plane, with the outer normal oriented outwards
%   of the polyhedron.
%
%   Typical values for classical polyhedra are:
%     cube side a               breadth = (3/2)*a
%     cuboid sides a, b, c      breadth = (a+b+c)/2
%     tetrahedron side a        breadth = 0.9123*a
%     octaedron side a          beradth = 1.175*a
%     dodecahedron, side a      breadth = 15*arctan(2)*a/(2*pi)
%     icosaehdron, side a       breadth = 15*arcsin(2/3)*a/(2*pi)
%
%   Example
%   [v e f] = createCube;
%   polyhedronMeanBreadth(v, e, f)
%   ans =
%       1.5
%
%   See also
%   meshes3d, meshEdgeFaces, meshDihedralAngles, checkMeshAdjacentFaces
%   trimeshMeanBreadth
%
%   References
%   Stoyan D., Kendall W.S., Mecke J. (1995) "Stochastic Geometry and its
%       Applications", John Wiley and Sons, p. 26
%   Ohser, J., Muescklich, F. (2000) "Statistical Analysis of
%       Microstructures in Materials Sciences", John Wiley and Sons, p.352

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2010-10-04,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% compute dihedral angle of each edge
alpha = meshDihedralAngles(vertices, edges, faces);

% compute length of each edge
lengths = meshEdgeLength(vertices, edges);

% compute product of length by angles
breadth = sum(alpha.*lengths)/(4*pi);


function w = map ( code, element_order )

%*****************************************************************************80
%
%% MAP returns the interpolation matrix for any available element.
%
%  Formula:
%
%    For an element of order ELEMENT_ORDER, we suppose we are given
%    ELEMENT_ORDER items of data Q associated with the nodes.
%
%   Let PHI(J)(R,S) be the Lagrange basis polynomial associated with
%   node J.  PHI(J)(R,S) is 1 at node J, and 0 at each of the other nodes.
%
%   Let P(R,S) be the polynomial of ELEMENT_ORDER terms which interpolates the
%   data Q, that is,
%
%      P(R(J),S(J)) = Q(J)
%
%   where the coordinates of node J are (R(J),S(J)).  Then we know
%   that we can write
%
%     P(R,S) = sum ( 1 <= J <= ELEMENT_ORDER ) Q(J) * PHI(J)(R,S)
%
%   But P(R,S) also has a standard representation as
%
%     P(R,S) = sum ( 1 <= I <= ELEMENT_ORDER ) A(I) * R**REXP(I) * S**SEXP(I)
%
%   where REXP(I) and SEXP(I) are the exponents of R and S and
%   the A(I) are the appropriate coefficients.
%
%   The interpolation matrix W allows us to immediately compute
%   the standard basis coefficients A from the data Q to be interpolated
%   using the formula:
%
%      A(I) = sum ( 1 <= J <= ELEMENT_ORDER ) W(I,J) * Q(J)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character CODE(*), identifies the element.
%    Legal values include 'Q4', 'Q8', 'Q9', 'Q12', 'Q16', 'QL',
%    'T3', 'T6' and 'T10'.
%
%    Input, integer ELEMENT_ORDER, the order associated with the code.
%
%    Output, real W(ELEMENT_ORDER,ELEMENT_ORDER), the interpolation matrix.
%

%
%  Get the (R,S) location of the nodes.
%
  [ r, s, area ] = node_reference ( code );
%
%  Get the associated monomials.
%
  [ rexp, sexp ] = poly ( code );
%
%  Set up the Vandermonde matrix.
%  Factors of the form 0**0 are to be understood as 1.
%
  for i = 1 : element_order
    for j = 1 : element_order

      if ( rexp(j) == 0 )
        rfact = 1.0;
      else
        rfact = r(i)^rexp(j);
      end

      if ( sexp(j) == 0 )
        sfact = 1.0;
      else
        sfact = s(i)^sexp(j);
      end

      w(i,j) = rfact * sfact;

    end
  end
%
%  Factor the Vandermonde matrix.
%
  [ w_lu, pivot, info ] = r8ge_fa ( element_order, w );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MAP - Fatal error!\n' );
    fprintf ( 1, '  The Vandermonde matrix is singular.\n' );
    error ( 'MAP - Fatal error!' );
  end
%
%  Invert the Vandermonde matrix.
%
  w = r8ge_inverse ( element_order, w_lu, pivot );

  return
end


function result=halfspacedepth(u,v,x,y)

%HALFSPACEDEPTH computes the halfspace depth of a (two-dimensional) point theta
% relative to a bivariate data set.
%
% The algoritm is described in:
%    Rousseeuw, P., Ruts, I. (1996),
%    "AS 307: Bivariate Location Depth",
%    Applied Statistics (JRSS-C), 45, 516-526.
%
% Required input arguments:
%            u : first coordinate of the point theta
%            v : second coordinate of the point theta
%            x : vector containing the first coordinates of all the data
%                points
%            y : vector containing the second coordinates of all the data
%                points
%
% I/O: [result] = halfspacedepth(u,v,x,y);
%
% This function is part of the Matlab Library for Robust Analysis,
% available at:
%           http://wis.kuleuven.be/stat/robust.html
%
% Written by Fabienne Verwerft
%
%
% Checking input
%
if not(length(x)==length(y))
    error('The vectors x and y must have the same length.')
end
if sum(isnan(x))>=1 || sum(isnan(y))>=1
    error('Missing values are not allowed')
end
%
% m is the number of data points that coincide with theta
%
eps=0.000001;
n=length(x);
norm=sqrt((x-u).^2 + (y-v).^2);
m=sum(norm<=eps);
ll=find(norm>eps);
norm=norm(ll);
xn=(x(ll)-u)./norm;
yn=(y(ll)-v)./norm;
%
% The vector containes the indices of the elements of x and y that satisfy
% the conditions.
%
k=find((abs(x(ll))>abs(y(ll))) & (xn>=0));
alfa(k)=asin(yn(k));
t=find(alfa(k)<0);
alfa(k(t))=2*pi+alfa(k(t));
k=find((abs(x(ll))>abs(y(ll))) & (xn<0));
alfa(k)=pi-asin(yn(k));
k=find((abs(x(ll))<=abs(y(ll))) & (yn>=0));
alfa(k)=acos(xn(k));
k=find((abs(x(ll))<=abs(y(ll))) & (yn<0));
alfa(k)=2*pi-acos(xn(k));
g=find(alfa>=(2*pi-eps));
alfa(g)=0;
%
nn=n-m;
if nn <= 1
    hdepth=m;
    result=hdepth;
    return
    % all data points coincide with theta
end
%
alfa=sort(alfa);
%
hoek=max(alfa(1)-alfa(nn)+2*pi,max(diff(alfa)));
if hoek > pi+eps
    hdepth=m;
    result=hdepth;
    return
    % hdepth=0 because theta lies outside the datacloud
end
%
% rotation around theta
% nu is the number of angles in the upper halfcircle
%
alfa=alfa-alfa(1);
nu=sum(alfa < (pi-eps));

if nu >= nn
    hdepth=m;
    result=hdepth;
    return
    % hdepth=0 every angle in the upper halfcircle so theta outside the
    % datacloud.
end
%
% construction of the array F
%
beta=alfa+pi;
alfatwee=alfa+2*pi;
A=[alfa,alfatwee,beta];
Aindex(1:2*nn)=1;
Aindex((2*nn+1):3*nn)=0;
[As,Asin]=sort(A);
Aindexs=Aindex(Asin);
pp=cumsum(Aindexs);
juisten=find(Aindexs==0);
F=pp(juisten);
%
% Adjust the array F for the angles that coincide with beta.
%
gelijkab=intersect(find(diff(As)<=eps)+1,juisten);
betagelijka=Asin(gelijkab);
if length(gelijkab)>0
    for i=1:length(gelijkab)
        aantal=sum((As(Aindexs==1)+eps)<As(gelijkab(i)));
        F(betagelijka(i)-2*nn)=aantal;
    end
end
%
gindex=find(diff(alfa)==0)+1;
G=0:(nn-1);
%
% Adjust the array G for angles alfa who coincide
%
if length(gindex)>0
    for i=1:length(gindex)
        aantal=sum(alfa(1:gindex(i))<alfa(gindex(i)));
        G(gindex(i))=aantal;
    end
end
%
k=F-G;
numh=min(min(k,(nn-k)));
result=(numh+m);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes the angular displacements of a damped pendulum under
%           gravity
%
% PURPOSE: to illustrate that changing the pendulum bob's radius results in
%          significant dynamical differences in the pendulum system
%
% Author: N.A. Battista
% Date: 12/13/2019
% Institution: The College of New Jersey
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function damped_pendulum_diff_r()

%
% Physical Parameters
%
m = 100;    % Mass of Pendulum bob
g=9.80665;  % Gravitational Acceleration;
L=0.2;      % Length of Pendulum

%
% Radii of Bobs (here - all different)
%
rVec = 100*[0.0025 0.005 0.01 0.015 0.02 0.025];

%
% Damping Parameter (here - all the same)
%
bVec = 50*ones(1,6);

%
% Temporal Parameters
%
t=0;        % initial time
tFinal = 24; % final-time
dt = 1e-3;  % time-step
ct=1;       % counter
tVec(ct)=t; % time vector storage

%
% Moment of Inertia
%
IVec = m*(0.5*rVec.^2+L^2);

%
% Initial Angular Displacement
%
ang = -(pi/2-pi/5)*ones(1,6);
phi = zeros( size( ang ) );

%
% Perform the time-stepping, e.g., solve the ODEs
%
while t<=tFinal

    % increment the storage counter
    ct = ct+1;

    % increment time
    t = t+dt;

    % save current time
    tVec(ct) = t;

    % solve 1st order ODE for angular displacement
    ang(ct,:) = ang(ct-1,:) + dt * ( phi(ct-1,:) );

    % solve 1st order ODE for angular velocity
    phi(ct,:) = phi(ct-1,:) + dt * ( -m*g*L ./IVec .* sin( ang(ct-1,:) ) - bVec.*phi(ct-1,:)./IVec );

end

%
% Defining Colors for Plotting Data
%
color2 = [0.635 0.078 0.184];  % dark red
color5 = [0.9 0.525 0.098];    % orange
color6 = [0.929 0.840 0.1250]; % dark yellow
color7 = [0.7 0.7 0];          % 'ugly' green
color13 = [0 0.5 0.5];             % green

%
% Note discrepancy is a bit off due to small angle approximation:
% sin(theta) ~ theta gives below period of oscillation
%
T1 = 2*pi*sqrt( IVec(2) / (m*g*L) );
%
% Actually computed value of period is:
T1 = 1.93;

%
% Make Figure of Angular Displacement vs. Non-dimensional Time (# of oscillations of 'r' case)
%
lw=6;
fs=18;
plot(tVec/T1,ang(:,1),'k-','LineWidth',lw); hold on;
plot(tVec/T1,ang(:,2),'b-','LineWidth',lw,'Color',color2); hold on;
plot(tVec/T1,ang(:,3),'g-','LineWidth',lw,'Color',color5); hold on;
plot(tVec/T1,ang(:,4),'r-','LineWidth',lw,'Color',color6); hold on;
plot(tVec/T1,ang(:,5),'c-','LineWidth',lw,'Color',color7); hold on;
plot(tVec/T1,ang(:,6),'c-','LineWidth',lw,'Color',color13); hold on;
plot(tVec/T1,ang(:,5),'c-','LineWidth',lw,'Color',color7); hold on;
plot(tVec/T1,ang(:,4),'r-','LineWidth',lw,'Color',color6); hold on;
plot(tVec/T1,ang(:,3),'g-','LineWidth',lw,'Color',color5); hold on;
plot(tVec/T1,ang(:,2),'b-','LineWidth',lw,'Color',color2); hold on;
plot(tVec/T1,ang(:,1),'k-','LineWidth',lw); hold on;
xlabel('Non-Dimensional Time (# periods of r-case)');
ylabel('Angular Displacement (Radians)');
axis([0 12 -1 1.5]);
leg=legend('$\frac{1}{2}$r','r','2r','3r','4r','5r','Orientation','horizontal','interpreter','latex');
set(gca,'FontSize',fs);
set(leg,'FontSize',fs+2);
leg.NumColumns=3;


  function x = sinc_periodic(t, K)
%|function x = sinc_periodic(t, K)
%| periodic sinc function, obtained by replicates of a sinc:
%| x(t) = \sum_l sinc(t - l K) = ... = sin(pi*t) / tan(pi*t/K) / K for K even.
%| This function is bandlimited and its samples are an impulse train.
%| It is closely related to the Dirichlet function diric() for odd K,
%| but it differs for even K.
%| Copyright 2003-11-2, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if streq(t, 'test'), sinc_periodic_test, return, end

if ~rem(K,2) % even
	d = tan(pi*t/K);
	j = abs(d) > 1e-12;
	x = ones(size(t));
	t = t(j);
	d = d(j);
	x(j) = sin(pi*t) ./ d / K;
else
	x = nufft_diric(t, K, K, 1);
%	xo = diric(2*pi*t/K,K); % would require matlab's signal toolbox
%	max_percent_diff(xo, x)
end

%
% self test
%
function sinc_periodic_test

Klist = [4 5];
im clf, pl=220;
for kk=1:2
	K = Klist(kk);
	n = [0:(4*K)]';
	t = linspace(0,4*K,401)';
	x = @(t,K) sinc_periodic(t, K);
%	y = @(t,K) diric(2*pi*t/K,K); % would require matlab's signal toolbox
	y = @(t,K) nufft_diric(t,K,K,1);
	if im
		subplot(pl+kk+0)
		plot(t, x(t,K), '-', n, x(n,K), 'o')
		titlef('Sinc-Periodic K=%d', K)
		axis([0 4*K -1 1]), xtick([0:4]*K), grid
		subplot(pl+kk+2)
		plot(t, y(t,K), '-', n, y(n,K), 'o')
		titlef('Dirichlet K=%d', K)
		axis([0 4*K -1 1]), xtick([0:4]*K), grid
		ylabelf('K=%d', K)
	end
end


% solution of Poisson equation using sparse matrix
%
%  E.Holzbecher,    18.3.2011
%
%--------------------------------------------------
nx = 12; ny = 12;      % dimensions in x- and y-direction
h = 1/4;                % grid spacing

% boundary type indicators (1=Dirichlet, 0=Neumann no-flow)
ltop = logical(zeros(1,nx));         % top
lbottom = logical(zeros(1,nx));      % bottom
lleft = logical([ones(6,1) zeros(6,1)]);       % left
lright = logical([zeros(6,1) ones(6,1)]);      % right

% boundary values (Dirichlet only)
btop = ones(1,nx);                  % top
bbottom = zeros(1,nx);              % bottom
bleft = ones(ny,1);                 % left
bright = zeros(ny,1);               % right

q = 1*ones(nx,ny);                  % right hand side (source term)

N = nx*ny;
d = [-nx,-1,0,1,nx];
B = [ones(N,2) -4*ones(N,1) ones(N,2)];
q = reshape(q,N,1);
b = -h*h*q.*ones(N,1);
for i = 1:nx
    if ltop(i)
        b(i) = b(i)-btop(i);
    else
        B(i,3) = -3;
        %B(i-1,1) = 0;
    end
    if lbottom(i)
        b(N-nx+i) = b(N-nx+i)-bbottom(i);
    else
        B(N-nx+i,3) = -3;
        % B(N-nx+i,5) = 0;
    end
end
for i = 1:ny
    B(i*nx,2) = 0;
    if i<ny B(i*nx+1,4) = 0; end
    if lleft(i)
        b((i-1)*nx+1) = b((i-1)*nx+1)-bleft(i);
    else
        B((i-1)*nx+1,3) = B((i-1)*nx+1,3)+1;
    end
    if lright(i)
        b(i*nx) = b(i*nx)-bright(i);
    else
        B(i*nx,3) = B(i*nx,3)+1;
    end
end

A = spdiags(B,d,N,N);

% processing: solution
U = A\b;
U = reshape(U,nx,ny);

% check & visualize
4*del2(U)
surf (U)


function yp = p06_fun ( neqn, t, y )

%*****************************************************************************80
%
%% P06_FUN evaluates the function for problem P06.
%
%  Discussion:
%
%    y1' = 2 y1 * ( 1 - y2 )
%    y2' = - y2 * ( 1 - y1 )
%    y1(0) = 1
%    y2(0) = 3
%
%    2 equations.
%    Enright and Pryce nonstiff problem #B1.
%    Autonomous.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Wayne Enright, John Pryce,
%    Algorithm 648,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 1, pages 28-34.
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the derivative
%    function.
%
%    Output, real YP(NEQN), the value of the derivative function.
%
  yp = zeros ( neqn, 1 );

  yp(1) = 2.0 * y(1) * ( 1.0 - y(2) );
  yp(2) =     - y(2) * ( 1.0 - y(1) );

  return
end


function n = tetrahedron_unit_lattice_point_num_3d ( s )

%*****************************************************************************80
%
%% TETRAHEDRON_UNIT_LATTICE_POINT_NUM_3D counts lattice points.
%
%  Discussion:
%
%    The tetrahedron is assumed to be the unit tetrahedron:
%
%    ( (0,0,0), (1,0,0), (0,1,0), (0,0,1) )
%
%    or a copy of this tetrahedron scaled by an integer S:
%
%    ( (0,0,0), (S,0,0), (0,S,0), (0,0,S) ).
%
%    The routine returns the number of integer lattice points that appear
%    inside the tetrahedron, or on its faces, edges or vertices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 July 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Matthias Beck, Sinai Robins,
%    Computing the Continuous Discretely,
%    Springer, 2006,
%    ISBN13: 978-0387291390,
%    LC: QA640.7.B43.
%
%  Parameters:
%
%    Input, integer S, the scale factor.
%
%    Output, integer N, the number of lattice points.
%
  n = ( ( s + 3 ) * ( s + 2 ) * ( s + 1 ) ) / 6;

  return
end


%The purpose of this program is to implement Galerkin method over
% the entire domain for solving the following general 2nd order,
% homogeneous, Boundary Value problem (BVP) with constant coefficients, and
% then comparing the answer with the exact solution.
%==========================================================================
%               ax"(t)+bx'(t)+cx(t)=0 for t1<=t<=t2
%                   BC: x(t1)=x1 and x(t2)=x2
% >> BVP_Galerkin1(a,b,c,t1,t2,x1,x2,n)
% where "n" is the number of trial functions
% The output of this program is
% 1- The approximated solution of x(t) vs. exact solution of x(t)
% 2- The approximated x'(t) vs. exact x'(t)
% 3- The approximated x"(t) vs. exact x"(t)
% ======Example============================================================
% Equation                  x"(t)+ x'(t)+ x(t)=0
% Boundary values           x(1)=2,     x(10)=0;
% Solution: We have:
%                           a=1;    b=1;   c=1;
%                           t1=1;       t2=10;
%                           x1=2;       x2=0;
% Using n=8 tiral functions,
% >>BVP_Galerkin(1,2,3,1,10,2,0,8)
%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%                   October.26, 2010, Ramin Shamshiri
%                       ramin.sh@ufl.edu
%            Doctoral student at the University of Florida
%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

function BVP_Galerkin1(a,b,c,t1,t2,x1,x2,n) % Declare function
dt=0.001; % increment
%% Begin Approximate solution via Galerkin method over the entire domain

for i=1:n
    for j=1:n
    K1(i,j)=(i*j)*(t2^(j+i-1)-t1^(j+i-1))/(i+j-1);          % K1 matrix corresponding to the x"(t)
    K2(i,j)=j*((t2^(i+j))-(t1^(i+j)))/(i+j);                % K2 matrix corresponding to the x'(t)
    K3(i,j)=(t2^(i+j+1)-(t1^(i+j+1)))/(i+j+1);              % K3 matrix corresponding to the x(t)
    end
end
% Overall Matrix
K=-a*K1+b*K2+c*K3;

%Applying boundary conditions and Solving nxn system of equations
for i=1:n-1
     for j=1:n
         A(i,j)=K(i,j)+((j*a*t2^(j+i-1))-(j*a*t1^(j+i-1)));

     end
end

%Applying boundary conditions
for j=1:n
    A(n-1,j)=(t1^j);
    A(n,j)=(t2^j);
end

for i=1:n-1
    B(i,1)=0;
end
B(n-1,1)=x1;
B(n,1)=x2;

C=inv(A)*B; % Ci: Coefficients of terms

t=t1:dt:t2;
t=t';
% Creating vertical axis values, x(t)
x=  zeros((t2-t1)/dt+1,1);
for i=1:(t2-t1)/dt+1
    for j=1:n
        x(i,1)=x(i,1)+ C(j,1)*(t(i,1)^j);
    end
end

% calculating x'(t)
dx=zeros((t2-t1)/dt+1,1);
for i=2:(t2-t1)/dt+1
    dx(i,1)=(x(i)-x(i-1))/dt;
end
% calculating x"(t)
d2x=zeros((t2-t1)/dt+1,1);
for i=3:(t2-t1)/dt+1
    d2x(i,1)=(dx(i)-dx(i-1))/dt;
end

 figure1 = figure('Color',[1 1 1]);
% Plotting approximate solution via Galerkin method over the entire domain
subplot(3,1,1,'Parent',figure1);
% Plotting the Approximate solution in green (Galerkin method over the entire domain)
plot(t,x,'-.', 'Color','r', 'LineWidth',2), grid on; hold on;
% Plotting the approximate dx
subplot(3,1,2,'Parent',figure1);
plot(t,dx, '-.', 'Color','r', 'LineWidth',2), grid on; hold on;
% Plotting the approximate d2u
subplot(3,1,3,'Parent',figure1);
plot(t,d2x, '-.', 'Color','r', 'LineWidth',2), grid on; hold on;
%% End of approximate solution via Galerkin method over the entire domain

%% ==============Begin Calculating Exact Soution==========================
r=roots([a b c]);   % Roots of auxiliary equation
r1=r(1);  r2=r(2);

% Bulding t-axis values
t=t1:dt:t2; t=t';
s=size(t);

% Bulding x(t) axis values
x=zeros(s(1),1);
if r1==r2   % For critically damped case

    C=inv([exp(r1*t1)  t1*exp(r2*t1);exp(r1*t2)  t2*exp(r2*t2)])*([x1;x2]);
    C1=C(1);
    C2=C(2);

    for i=1:s(1)
        x(i)=C1*exp(r1*t(i))+C2*t(i)*exp(r2*t(i));
    end

else        % For Under-damped or Over-damped case

    C=inv([exp(r1*t1) exp(r2*t1);exp(r1*t2) exp(r2*t2)])*([x1;x2]);
    C1=C(1);
    C2=C(2);

    for i=1:s(1)
        x(i)=C1*exp(r1*t(i))+C2*exp(r2*t(i));
    end
end

% calculating x'(t)
dx=zeros(s(1),1);
for i=2:s(1)
    dx(i,1)=(x(i)-x(i-1))/dt;
end

% calculating x"(t)
d2x=zeros(s(1),1);
for i=3:s(1)
    d2x(i,1)=(dx(i)-dx(i-1))/dt;
end

% Plotting Exact solution
subplot(3,1,1,'Parent',figure1);
% Plotting the exact solution x(t) in blue
plot(t,x, 'Color','b', 'LineWidth',2);
xlabel('Time (sec)','FontSize',12);
ylabel('x(t) (m)','FontSize',12);
legend1 = legend(subplot(3,1,1),'show');
legend('Approximate x(t)','Exact x(t)')

% Plotting the exact solution x'(t) in blue
subplot(3,1,2,'Parent',figure1);
plot(t,dx, 'Color','b', 'LineWidth',2);
xlabel('Time (sec)','FontSize',12);
ylabel('dx/dt (m/s)','FontSize',12);
legend1 = legend(subplot(3,1,2),'show');
legend('Approximate dx(t)','Exact dx(t)')

% Plotting the exact solution x"(t) in blue
subplot(3,1,3,'Parent',figure1);
plot(t,d2x, 'Color','b', 'LineWidth',2);
xlabel('Time (sec)','FontSize',12);
ylabel('d^2x/dt^2 m/s^2','FontSize',12);
legend1 = legend(subplot(3,1,3),'show');
legend('Approximate d2x(t)','Exact dx(t)')
clc
%% =====================End of exact solution==============================


function [S,C]=allsums(varargin)
% ALLSUMS Distribution of unique sums among all combinations of vectors.
%   [S,C]=ALLSUMS(IN) depending on input IN, will do one of two different
%   procedures for calculating all unique combinations of sums.
%
%   The output vectors, S and C, are respectively the list of unique
%   sums, and the count distribution for those sums among all combinations.
%
%
%   SINGLE INPUT:
%
%   [S,C]=ALLSUMS(VEC) for a single input vector of length N, returns the
%   distribution of unique sums among the (2^N)-1 combinations of
%   non-empty subsets of elements within VEC.
%
%    EXAMPLE:
%      Number of ways to have a total of 250 using only the odd numbers
%      from 1 to 100.
%      [S,C]=allsums(1:2:100);
%      num=C(S==250)
%
%
%   MULTIPLE INPUTS:
%
%   [S,C]=ALLSUMS(VEC,K) with vector VEC of length N, assumes the
%   equivalent scenario of rolling a N-sided die, with values VEC,
%   K times and returns the distributions of unique sums among all
%   N^K combinations of values.
%
%   [S,C]=ALLSUMS(VEC1,K1,VEC2,K2,...) with vectors of length N1, N2, ...
%   assumes the equivalent scenario of rolling a N1-sided die, with
%   values VEC1, K1 times; then rolling a N2-sided dice, with values VEC2,
%   K2 times; etc. and returns the distribution of unique sums among
%   all (N1^K1)*(N2^K2)*... combinations of values
%
%    SINGLE VECTOR, REPEATED
%      Probability distribution of rolling a regular 6-sided die 100 times
%      [S,C]=allsums(1:6,100);
%      plot(S,C./sum(C)), grid on
%
%    MULTIPLE VECTORS, REPEATED
%      Probability of the total yielding exactly zero when rolling a
%      A) six-sided die with values [-5:0] ten times, and then a
%      B) ten-sided die with values [-4:5] twenty times.
%      [S,C]=allsums([-5:0],10,[-4:5],20);
%      prob=C(S==0)/sum(C)
%

%   Mike Sheppard
%   Last Modified: 27-Oct-2011

%ERROR CHECKING
ninputs=numel(varargin);
switch ninputs
    case 0
        error('allsums:Inputs','Requires at least one input');
    case 1
        VEC=varargin{1}; %Input vector
        if any(~isfinite(VEC))
            error('allsums:Inputs','Requires input vector to be numeric');
        end
    otherwise
        num=ninputs/2;
        if mod(ninputs,2)~=0
            error('allsums:Inputs','Requires inputs to be in [Vector],[Number] order');
        end
        for k=1:num
            ALLVEC{k}=varargin{2*k-1}; ALLNUMR{k}=varargin{2*k}; numr=ALLNUMR{k};
            if any(~isfinite(ALLVEC{k}))
                error('allsums:Inputs','Requires all input vectors to be numeric');
            end
            if any([~isfinite(numr) ~isscalar(numr) numr~=round(numr) numr<=0])
                error('allsums:Inputs','Requires number of repetitions to be a positive scalar integer');
            end
        end
end

%ALGORITHM
switch ninputs
    case 1
       % Distribution of unique sums among the (2^N)-1 combinations of
       % non-empty subsets of elements within VEC.
        S=[]; C=[];
        for k=1:numel(VEC)
            [S,ign,indx]=unique([S VEC(k) S+VEC(k)]);
            C=accumarray(indx',[C 1 C])';
        end
        S=S(:); C=C(:);
    otherwise
        % Distribution of unique sums among all (N1^K1)*(N2^K2)*...
        % combinations of vectors of length N_k repeated K_k times,
        % with values VEC_k each.
        S=0; C=1;
        for k=1:num
            VEC=ALLVEC{k}; VEC=VEC(:); numr=ALLNUMR{k};
            for n=1:numr
                tempv=bsxfun(@plus,S',VEC);
                tempc=repmat(C',length(VEC),1);
                [S,ign,indx]=unique(tempv(:));
                C=accumarray(indx,tempc(:));
            end
        end
end

end


% <Purpose>
%    Computing the numerical rank of a downdated matrix.
%
% <Syntax>
%    [r,Basis,C] = NumericalRankDowndate(A,pth,C,RC)
%
% <Input Parameters>
%  1.   A -- the target matrix;
%  2. pth -- the index of row/column to be deleted;
%  3.   C -- cell array contains information required by updating/downdating;
%  4.  RC -- Set to 'row', then the pth row will be deleted.
%            Set to 'column', then the pth column will be deleted.
%
% <Output Parameters>
%  1. r     -- the numerical rank of the downdated matrix;
%  2. Basis --
%         For high rank cases...
%             a matrix whose columns form an orthonormal basis of
%             the numerical kernel;
%
%         For low rank cases...
%             a matrix whose columns form an orthonormal basis of
%             the numerical range;
%
%  3.     C -- Matlab cell array
%
%     For high rank cases...
%         C{1,1} = rank : the numerical rank of the downdated matrix;
%         C{2,1} = Basis : matrix whose columns form an orthonormal kernel basis;
%         C{3,1} = Q : the Q in the QR decomposition of the kernel stacked matrix;
%         C{4,1} = R : the R in the QR decomposition of the kernel stacked matrix;
%         C{5,1} = tau : scaling factor in the kernel stacked matrix;
%         C{6,1} = tol : the rank decision threshold;
%
%     For low rank cases...
%         C{1,1} = rank : the numerical rank of the downdated matrix;
%         C{2,1} = U : the U in the USV+E decomposition of the downdated matrix;
%         C{3,1} = V : the V in the USV+E decomposition of the downdated matrix;
%         C{4,1} = S : the S in the USV+E decomposition of the downdated matrix;
%         C{5,1} = tol : the rank decision threshold;
%
% <Reference>
%   [1] T.Y. Li and Z. Zeng, "A Rank-Revealing Method with Updating, Downdating
%       and Applications", SIAM J. Matrix Anal. and Appl., 26 (2005), pp. 918--946.
%
%   [2] T.L. Lee, T.Y. Li and Z. Zeng, "A Rank-Revealing Method with Updating,
%       Downdating and Applications, Part II", SIAM J. Matrix Anal. and Appl. (2009).


function value = r8vec_length ( dim_num, x )

%*****************************************************************************80
%
%% R8VEC_LENGTH returns the Euclidean length of an R8VEC
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 August 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real X(DIM_NUM), the vector.
%
%    Output, real VALUE, the Euclidean length of the vector.
%
  value = sqrt ( sum ( ( x(1:dim_num) ).^2 ) );

  return
end


function value = g_hofstadter ( n )

%*****************************************************************************80
%
%% G_HOFSTADTER computes the Hofstadter G sequence.
%
%  Discussion:
%
%    G(N) = 0                      if N = 0
%         = N - G ( G ( N - 1 ) ), otherwise.
%
%    G(N) is defined for all nonnegative integers.
%
%    The value of G(N) turns out to be related to the Zeckendorf
%    representation of N as a sum of non-consecutive Fibonacci numbers.
%    To compute G(N), determine the Zeckendorf representation:
%
%      N = sum ( 1 <= I <= M ) F(I)
%
%    and reduce the index of each Fibonacci number by 1:
%
%      G(N) = sum ( 1 <= I <= M ) F(I-1)
%
%    However, this is NOT how the computation is done in this routine.
%    Instead, a straightforward recursive function call is defined
%    to correspond to the definition of the mathematical function.
%
%  Table:
%
%     N  G(N)  Zeckendorf   Decremented
%    --  ----  ----------   -----------
%
%     1   1    1            1
%     2   1    2            1
%     3   2    3            2
%     4   3    3 + 1        2 + 1
%     5   3    5            3
%     6   4    5 + 1        3 + 1
%     7   4    5 + 2        3 + 1
%     8   5    8            5
%     9   6    8 + 1        5 + 1
%    10   6    8 + 2        5 + 1
%    11   7    8 + 3        5 + 2
%    12   8    8 + 3 + 1    5 + 2 + 1
%    13   8    13           8
%    14   9    13 + 1       8 + 1
%    15   9    13 + 2       8 + 1
%    16  10    13 + 3       8 + 2
%    17  11    13 + 3 + 1   8 + 2 + 1
%    18  11    13 + 5       8 + 3
%    19  12    13 + 5 + 1   8 + 3 + 1
%    20  12    13 + 5 + 2   8 + 3 + 1
%    21  13    21           13
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Douglas Hofstadter,
%    Goedel, Escher, Bach,
%    Basic Books, 1979.
%
%  Parameters:
%
%    Input, integer N, the argument of the function.
%
%    Output, integer VALUE, the value of the function.
%
  if ( n <= 0 )
    value = 0;
  else
    value = n - g_hofstadter ( g_hofstadter ( n-1 ) );
  end

  return
end


% Isoparametric Formulation Implementation
% clear memory
clear all
close all
clc
% E: modulus of elasticity
% A: area of cross section
% L: length of bar
E=8; L=4;
u_exact=@(x) (56-8*(x-2)-24*heaviside(x-5))/2/x;
hold on;
ezplot(u_exact,[2 6])
title('Exact Solution v.s. FEM','interpreter','latex');
xlabel('x','interpreter','latex');
ylabel('Axial stress, $\it{\sigma}_{x}$','interpreter','latex','FontSize',12);

for i=1:4   %For NEL = 1, 2, 4, 8
fprintf( '\nNumber of elements:%d\n\n',2^(i-1) );
% numberElements: number of elements
numberElements=2^(i-1);
% numberNodes: number of nodes
numberNodes=2*numberElements+1;
A=zeros(1,numberElements);
% generation of coordinates and connectivities
NNOD=3;
nodeCoordinates=linspace(2,L+2,numberNodes);
%Generate element length vector
for i=1:numberElements
    Le(i)=L/numberElements;
    elementNodes(i,:)=[(i-1)*2+1 (i-1)*2+2 (i-1)*2+3];
    A(i)=(nodeCoordinates(2*i-1)+Le(i)/2)*2;
end
% for structure:
   % displacements: displacement vector
   % force : force vector
   % stiffness: stiffness matrix
force=zeros(numberNodes,1);
stiffness=zeros(numberNodes,numberNodes);
% computation of the system stiffness matrix and force vector
for e=1:numberElements;
  % elementDof: element degrees of freedom (Dof)
  elementDof=elementNodes(e,:) ;
  detJacobian=Le(e)/2;
  invJacobian=1/detJacobian;
  ngp = 3;
  [w,xi]=gauss1d(ngp);
  xc=0.5*(nodeCoordinates(elementDof(1))+nodeCoordinates(elementDof(end)));
  for ip=1:ngp;
      [shape,naturalDerivatives]=shapeFunctionL3(xi(ip));
      B=naturalDerivatives*invJacobian;
      stiffness(elementDof,elementDof)=...
      stiffness(elementDof,elementDof)+ B'*B*w(ip)*detJacobian*E*A(e);
      force(elementDof)=force(elementDof)+...
          8*shape'*detJacobian*w(ip);
  end
  if(nodeCoordinates(elementDof(end))==5)
     x=(5-xc)/detJacobian;
     [s,n]=shapeFunctionL3(x);
     force(elementDof)= force(elementDof)+...
         24*s';
  end
  if(nodeCoordinates(elementDof(1))<5&&...
          nodeCoordinates(elementDof(3))>5)
     x=(5-xc)/detJacobian;
     [s,n]=shapeFunctionL3(x);
     force(elementDof)= force(elementDof)+...
         24*s';
  end
end
% boundary conditions and solution
% prescribed dofs
prescribedDof=[1];
% solution
GDof=numberNodes;
displacements=solution(GDof,prescribedDof,stiffness,force);
% output displacements/reactions
outputDisplacementsReactionsPretty(displacements,stiffness, ...
    numberNodes,prescribedDof,force)
fprintf('Axial stress\n')
fprintf('element\t\taxial stress\n')
%Stress and strain recovery
ngp = 3;
elementNodeCoor=zeros(numberElements*NNOD,1);
elementNodeStr=zeros(numberElements*NNOD,1);
for e=1:numberElements;
  % elementDof: element degrees of freedom (Dof)
  elementDof=elementNodes(e,:);
  detJacobian=Le(e)/2;
  invJacobian=1/detJacobian;
  xi=[-1 0 1];
  for ip=1:NNOD;
      [shape,naturalDerivatives]=shapeFunctionL3(xi(ip));
      B=naturalDerivatives*invJacobian;
      elementNodeCoor(ip+(e-1)*NNOD,1)=nodeCoordinates(elementDof(ip));
      elementNodeStr(ip+(e-1)*NNOD,1)=E*B*displacements(elementDof,1);
      fprintf('%2.0f\t%2.0fth node\t%10.4e\n', e, ip,elementNodeStr(ip+(e-1)*NNOD,1))
  end
end

%post process
switch numberElements
    case 1
        str='r*--';
    case 2
        str='g*--';
    case 4
        str='k*--';
    case 8
        str='m*--';
end

plot(elementNodeCoor,elementNodeStr,str)
hold on;
end
legend('Exact solution','NEL=1','NEL=2','NEL=4','NEL=8','interpreter','latex');


function [H A b] = affineLS(X1, X2, normalization)

% [H A b] = affineLS(X1, X2, normalization)
%
% DESC:
% computes the affine transformation between the point pairs X1, X2
%
% VERSION:
% 1.0.0
%
% INPUT:
% X1, X2        = point matches (cartesian coordinates)
% normalization = true (default) or false to enable/disable point
%                 normalzation
%
% OUTPUT:
% H             = homography representing the affine transformation
% A             = 4x4 affine coefficient matrix
% b             = affine translation vector

% AUTHOR:
% Marco Zuliani, email: marco.zuliani@gmail.com
% Copyright (C) 2010 by Marco Zuliani
%
% LICENSE:
% This toolbox is distributed under the terms of the GNU GPL.
% Please refer to the files COPYING.txt for more information.

% HISTORY
% 1.0.0         11/22/10 - intial version

if (nargin < 3)
    normalization = true;
end;

N = size(X1, 2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% checks
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (size(X2, 2) ~= N)
    error('AffineLS:inputError', ...
        'The set of input points should have the same cardinality')
end;
if N < 2
    error('AffineLS:inputError', ...
        'At least 3 point correspondences are needed')
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% normalize the input
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if normalization
    % fprintf('\nNormalizing...')
    [X1, T1] = normalize_points(X1);
    [X2, T2] = normalize_points(X2);
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% estimation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Traditional LS
P = zeros(2*N, 6);
q = zeros(2*N, 1);

ind = 1:2;
for n = 1:N

    P(ind, :) = [X1(1,n) 0 X1(2,n) 0 1 0; 0 X1(1,n) 0 X1(2,n) 0 1];

    q(ind) = X2(1:2, n);

    ind = ind + 2;

end;

% solve the linear system in a least square sense
Theta = P\q;

% compute the corresponding homography
H = [Theta(1) Theta(3) Theta(5); Theta(2) Theta(4) Theta(6); 0 0 1];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% de-normalize the parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if normalization
    H = T2\H*T1;
end;
H = H/H(9);

% prepare the output
if nargout > 1

    A = H(1:2, 1:2);
    b = H(1:2, 3);

end;

return


function centroid = triangle_centroid_3d ( t )

%*****************************************************************************80
%
%% TRIANGLE_CENTROID_3D computes the centroid of a triangle in 3D.
%
%  Discussion:
%
%    The centroid of a triangle can also be considered the
%    center of gravity or center of mass, assuming that the triangle
%    is made of a thin uniform sheet of massy material.
%
%    The centroid of a triangle is the intersection of the medians.
%    A median of a triangle is a line connecting any vertex to the
%    midpoint of the opposite side.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(3,3), the triangle vertices.
%
%    Output, real CENTROID(3,1), the coordinates of the centroid.
%
  dim_num = 3;

  for i = 1 : dim_num
    centroid(i,1) = sum ( t(i,1:3) ) / 3.0;
  end

  return
end


function [D,P] = dijk(A,s,t)
%DIJK Shortest paths from nodes 's' to nodes 't' using Dijkstra algorithm.
% [D,p] = dijk(A,s,t)
%     A = n x n node-node weighted adjacency matrix of arc lengths
%         (Note: A(i,j) = 0   => Arc (i,j) does not exist;
%                A(i,j) = NaN => Arc (i,j) exists with 0 weight)
%     s = FROM node indices
%       = [] (default), paths from all nodes
%     t = TO node indices
%       = [] (default), paths to all nodes
%     D = |s| x |t| matrix of shortest path distances from 's' to 't'
%       = [D(i,j)], where D(i,j) = distance from node 'i' to node 'j'
%     P = |s| x n matrix of predecessor indices, where P(i,j) is the
%         index of the predecessor to node 'j' on the path from 's(i)' to 'j'
%         (use PRED2PATH to convert P to paths)
%       = path from 's' to 't', if |s| = |t| = 1
%
%  (If A is a triangular matrix, then computationally intensive node
%   selection step not needed since graph is acyclic (triangularity is a
%   sufficient, but not a necessary, condition for a graph to be acyclic)
%   and A can have non-negative elements)
%
%  (If |s| >> |t|, then DIJK is faster if DIJK(A',t,s) used, where D is now
%   transposed and P now represents successor indices)
%
%  (Based on Fig. 4.6 in Ahuja, Magnanti, and Orlin, Network Flows,
%   Prentice-Hall, 1993, p. 109.)

% Copyright (c) 1998-2001 by Michael G. Kay
% Matlog Version 5 22-Aug-2001

% Input Error Checking ******************************************************
error(nargchk(1,3,nargin));

[n,cA] = size(A);

if nargin < 2 | isempty(s), s = (1:n)'; else s = s(:); end
if nargin < 3 | isempty(t), t = (1:n)'; else t = t(:); end

if ~any(any(tril(A) ~= 0))       % A is upper triangular
   isAcyclic = 1;
elseif ~any(any(triu(A) ~= 0))   % A is lower triangular
   isAcyclic = 2;
else                             % Graph may not be acyclic
   isAcyclic = 0;
end

if n ~= cA
   error('A must be a square matrix');
elseif ~isAcyclic & any(any(A < 0))
   error('A must be non-negative');
elseif any(s < 1 | s > n)
   error(['''s'' must be an integer between 1 and ',num2str(n)]);
elseif any(t < 1 | t > n)
   error(['''t'' must be an integer between 1 and ',num2str(n)]);
end
% End (Input Error Checking) ************************************************

A = A';    % Use transpose to speed-up FIND for sparse A

D = zeros(length(s),length(t));
if nargout > 1, P = zeros(length(s),n); end

for i = 1:length(s)
   j = s(i);

   Di = Inf*ones(n,1); Di(j) = 0;

   isLab = logical(zeros(length(t),1));
   if isAcyclic ==  1
      nLab = j - 1;
   elseif isAcyclic == 2
      nLab = n - j;
   else
      nLab = 0;
      UnLab = 1:n;
      isUnLab = logical(ones(n,1));
   end

   while nLab < n & ~all(isLab)
      if isAcyclic
         Dj = Di(j);
      else	% Node selection
         [Dj,jj] = min(Di(isUnLab));
         j = UnLab(jj);
         UnLab(jj) = [];
         isUnLab(j) = 0;
      end

      nLab = nLab + 1;
      if length(t) < n, isLab = isLab | (j == t); end

      [jA,kA,Aj] = find(A(:,j));
      Aj(isnan(Aj)) = 0;

      if isempty(Aj), Dk = Inf; else Dk = Dj + Aj; end

      if nargout > 1, P(i,jA(Dk < Di(jA))) = j; end
      Di(jA) = min(Di(jA),Dk);

      if isAcyclic == 1       % Increment node index for upper triangular A
         j = j + 1;
      elseif isAcyclic == 2   % Decrement node index for lower triangular A
         j = j - 1;
      end
   end
   D(i,:) = Di(t)';
end

if nargout > 1 & length(s) == 1 & length(t) == 1
   P = pred2path(P,s,t);
end


function varargout = cylinderMesh(cyl, varargin)
%CYLINDERMESH  Create a 3D mesh representing a cylinder
%
%   [V F] = cylinderMesh(CYL)
%   Computes vertex coordinates and face vertex indices of a mesh
%   representing a 3D cylinder given as [X1 Y1 Z1 X2 Y2 Z2 R].
%
%   Example
%     % Draw a rotated cylinder
%     cyl = [0 0 0 10 20 30 5];
%     [v f] = cylinderMesh(cyl);
%     figure;drawMesh(v, f, 'FaceColor', 'r');
%     view(3); axis equal;
%
%     % Draw three mutually intersecting cylinders
%       p0 = [30 30 30];
%       p1 = [90 30 30];
%       p2 = [30 90 30];
%       p3 = [30 30 90];
%       [v1 f1] = cylinderMesh([p0 p1 25]);
%       [v2 f2] = cylinderMesh([p0 p2 25]);
%       [v3 f3] = cylinderMesh([p0 p3 25]);
%       figure; hold on;
%       drawMesh(v1, f1, 'FaceColor', 'r');
%       drawMesh(v2, f2, 'FaceColor', 'g');
%       drawMesh(v3, f3, 'FaceColor', 'b');
%       view(3); axis equal
%       set(gcf, 'renderer', 'opengl')
%
%   See also
%     drawCylinder, torusMesh, sphereMesh

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2012-10-25,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2012 INRA - Cepia Software Platform.

% extract cylinder data
p1 = cyl(:, 1:3);
p2 = cyl(:, 4:6);
r  = cyl(:, 7);

% compute length and orientation
[theta, phi, rho] = cart2sph2d(p2 - p1);

% parametrisation on x
t = linspace(0, 2*pi, 20);
lx = r * cos(t);
ly = r * sin(t);

% parametrisation on z
lz = linspace(0, rho, 10);

% generate surface grids
x = repmat(lx, [length(lz) 1]);
y = repmat(ly, [length(lz) 1]);
z = repmat(lz', [1 length(t)]);

% transform points
trans   = localToGlobal3d(p1, theta, phi, 0);
[x, y, z] = transformPoint3d(x, y, z, trans);

% convert to FV mesh
[vertices, faces] = surfToMesh(x, y, z, 'xPeriodic', true);

% format output
varargout = formatMeshOutput(nargout, vertices, faces);


function [ area, radout, side ] = polygon_inrad_data_2d ( n, radin )

%*****************************************************************************80
%
%% POLYGON_INRAD_DATA_2D determines polygonal data from its inner radius in 2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sides of the polygon.
%    N must be at least 3.
%
%    Input, real RADIN, the inner radius of the polygon, that is,
%    the radius of the largest circle that can be inscribed within
%    the polygon.
%
%    Output, real AREA, the area of the regular polygon.
%
%    Output, real RADOUT, the outer radius of the polygon, that is,
%    the radius of the smallest circle that can be described about
%    the polygon.
%
%    Output, real SIDE, the length of one side of the polygon.
%
  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'POLYGON_INRAD_DATA_2D - Fatal error!\n' );
    fprintf ( 1, '  Input value of N must be at least 3\n' );
    fprintf ( 1, '  but your input value was N = %d\n', n );
    error ( 'POLYGON_INRAD_DATA_2D - Fatal error!' );
  end

  angle = pi / n;
  area = n * radin * radin * tan ( angle );
  side = 2.0 * radin * tan ( angle );
  radout = 0.5 * side / sin ( angle );

  return
end


function value = i4_bit_lo1 ( n )

%*****************************************************************************80
%
%% I4_BIT_LO1 returns the position of the low 1 bit base 2 in an integer.
%
%  Example:
%
%       N    Binary    Lo 1
%    ----    --------  ----
%       0           0     0
%       1           1     1
%       2          10     2
%       3          11     1
%       4         100     3
%       5         101     1
%       6         110     2
%       7         111     1
%       8        1000     4
%       9        1001     1
%      10        1010     2
%      11        1011     1
%      12        1100     3
%      13        1101     1
%      14        1110     2
%      15        1111     1
%      16       10000     5
%      17       10001     1
%    1023  1111111111     1
%    1024 10000000000    11
%    1025 10000000001     1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the integer to be measured.
%    N should be nonnegative.
%
%    Output, integer I4_BIT_LO1, the position of the low 1 bit.
%
  bit = 0;
  i = n;

  while ( 1 )

    bit = bit + 1;
    i2 = floor ( i / 2 );

    if ( i ~= 2 * i2 )
      break
    end

    i = i2;

  end

  value = bit;

  return
end


function jacobi_eigenvalue_test03 ( )

%*****************************************************************************80
%
%% JACOBI_EIGENVALUE_TEST03 uses a 5x5 test matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 July 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'JACOBI_EIGENVALUE_TEST03\n' );
  fprintf ( 1, '  For a symmetric matrix A,\n' );
  fprintf ( 1, '  JACOBI_EIGENVALUE computes the eigenvalues D\n' );
  fprintf ( 1, '  and eigenvectors V so that A * V = D * V.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use the discretized second derivative matrix.\n' );

  n = 5;

  a = zeros ( n, n );
  for i = 1 : n
    a(i,i) = -2.0;
  end
  for i = 1 : n - 1
    a(i,i+1) = +1.0;
    a(i+1,i) = +1.0;
  end

  r8mat_print ( n, n, a, '  Input matrix A:' );

  it_max = 100;

  [ v, d, it_num, rot_num ] = jacobi_eigenvalue ( n, a, it_max );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations = %d\n', it_num );
  fprintf ( 1, '  Number of rotations  = %d\n', rot_num );

  r8vec_print ( n, d, '  Eigenvalues D:' );

  r8mat_print ( n, n, v, '  Eigenvector matrix V:' );
%
%  Compute eigentest.
%
  error_frobenius = r8mat_is_eigen_right ( n, n, a, v, d );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Frobenius norm error in eigensystem A*V-D*V = %g\n', ...
    error_frobenius );

  return
end


function out = proj_l1ball_box(x,w,r,u)
%PROJ_L1BALL_BOX computes the orthogonal projection onto the intersection of an l1 ball and a box
%                                                  {x: norm(w(:).*x(:),1)<=r, -u<=x<=u}
%
%  Usage:
%  out = PROJ_L1BALL_BOX(x,[w],[r],[u])
%  =============================================================
%  INPUT:
%  x - point to be projected (vector/matrix)
%  w - weights vecotr - [default: 1]
%  r - positive scalar - [default: 1]
%  u - box paramters (scalar/vector/matrix)- [default: Inf]
%  ==============================================================
%  Assumptions:
%  r,u >= 0
%  w >= 0
%  ==============================================================
%  Output:
%  out - projection vector

% This file is part of the FOM package - a collection of first order methods for solving convex optimization problems
% Copyright (C) 2017 Amir and Nili Beck
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

%reading the user x and setting defalut values when required.
if (nargin < 1)
    error ('usage: proj_l1_inter_box(x,[w],[r],[u])') ;
end

if (nargin < 4)
    %box parameters are not given, setting to defalut value :inf
    u = inf*(ones(size(x))) ;
end

if ((nargin < 3) || (isempty(r)))
    %ball radius is not given, setting to defalut value :1
    r = 1;
end

if ((nargin < 2) || (isempty(w)))
    %weight vector is not given, setting to default value: ones
    w=ones(size(x)) ;
end

if  ((r < 0) || (min(min(u)) < 0))
      error('Set is infeasible') ;
end

if (min(min(w)) < 0)
    error('usage: proj_l1_inter_box(x,[w],[r],[u]) - w should be a non-negative vector or matrix');
end

%checking the simple projection
out = min(max(x,-u),u) ;
if ((sum(sum(w .* abs (out)))) <= r)
    return;
end

%defining f on lambda - to be used by the bisetion

f= @(lam)   trace(w' * abs(min(max(abs(x)-lam*w,0),u) .* sign(x)))- r;
lambda_min = 0;

lambda_max = 1;
while(f(lambda_max)>0)
    lambda_max = lambda_max *2  ;
end

eps = 1e-10 ;
final_lam = bisection(f,lambda_min,lambda_max,eps) ;
out=  min(max(abs(x)-final_lam*w,0),u) .* sign(x) ;


function d = mahalanobis(varargin)
%MAHALANOBIS Computes the Mahalanobis distance.
%   D = MAHALANOBIS(Y, X) computes the Mahalanobis distance between
%   each vector in Y to the mean (centroid) of the vectors in X, and
%   outputs the result in vector D, whose length is size(Y, 1).  The
%   vectors in X and Y are assumed to be organized as rows.  The
%   input data can be real or complex. The outputs are real
%   quantities.
%
%   D = MAHALANOBIS(Y, CX, MX) computes the Mahalanobis distance
%   between each vector in Y and the given mean vector, MX. The
%   results are output in vector D, whose length is size(Y, 1).  The
%   vectors in Y are assumed to be organized as the rows of this
%   array. The input data can be real or complex. The outputs are
%   real quantities. In addition to the mean vector MX, the
%   covariance matrix CX of a population of vectors X also must be
%   provided. Use function COVMATRIX (Section 11.5) to compute MX and
%   CX.

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.6 $  $Date: 2005/01/18 13:44:47 $

% Reference: Acklam, P. J. [2002]. "MATLAB Array Manipulation Tips
% and Tricks." Available at
%     home.online.no/~pjacklam/matlab/doc/mtt/index.html
% or at
%     www.prenhall.com/gonzalezwoodseddins

param = varargin; % Keep in mind that param is a cell array.
Y = param{1};
ny = size(Y, 1); % Number of vectors in Y.

if length(param) == 2
   X = param{2};
   % Compute the mean vector and covariance matrix of the vectors
   % in X.
   [Cx, mx] = covmatrix(X);
elseif length(param) == 3 % Cov. matrix and mean vector provided.
   Cx = param{2};
   mx = param{3};
else
   error('Wrong number of inputs.')
end
mx = mx(:)'; % Make sure that mx is a row vector.

% Subtract the mean vector from each vector in Y.
Yc = Y - mx(ones(ny, 1), :);

% Compute the Mahalanobis distances.
d = real(sum(Yc/Cx.*conj(Yc), 2));


function ncomb = multinomial_coef2 ( nfactor, factor )

%*****************************************************************************80
%
%% MULTINOMIAL_COEF2 computes a Multinomial coefficient.
%
%  Discussion:
%
%    The multinomial coefficient is a generalization of the binomial
%    coefficient.  It may be interpreted as the number of combinations of
%    N objects, where FACTOR(1) objects are indistinguishable of type 1,
%    ... and FACTOR(NFACTOR) are indistinguishable of type NFACTOR,
%    and N is the sum of FACTOR(1) through FACTOR(NFACTOR).
%
%    NCOMB = N! / ( FACTOR(1)! FACTOR(2)! ... FACTOR(NFACTOR)! )
%
%    A direct method is used, which should be exact.  However, there
%    is a possibility of intermediate overflow of the result.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NFACTOR, the number of factors.
%    1 <= NFACTOR.
%
%    Input, integer FACTOR(NFACTOR), contains the factors.
%    0.0 <= FACTOR(I).
%
%    Output, integer NCOMB, the value of the multinomial coefficient.
%
  ncomb = 1;
  k = 0;

  for i = 1 : nfactor

    for j = 1 : factor(i)
      k = k + 1;
      ncomb = ( ncomb * k ) / j;
    end

  end

  return
end


%% example
function [Y, X, U] = example(U, varargin)

%% Syntax
%  function [Y, X, U] = example(U, varargin)

%% Description
% Function for simulating the nonlinear dynamic system:
%
%                y(k)
%   y(k+1) = ---------------  + [u(k)]^3
%            1 + y(k)*y(k)
%
% which is used to demonstrate the use of this toolbox.
% [1] K.S. Narendra and K. Parthasarathy. Identification
% and Control of Dynamical Systems Using Neural Networks,
% IEEE Transactions on NN, Vol.1 No. 1, 4-27, 1990.
%
% Usage:
% (1) [Y, X, U] = example(u, y0), u is vector, y0 is optional
% (2) [Y, X, U] = example(U, y0), U is Nx2 matrix
% Inputs:
% u .. vector of inputs
% U .. matrix U=[x u], where x is vector of past states nad u is vector of
%   inputs
% y0 .. intial state of the system, optional
% Outputs:
% Y .. vector of outputs
% X .. vector of past outputs: X(k) = Y(k-1)
% U .. vector of inputs
%
%

%% Examples
% demo_example_gp_data.m

%% See Also
% EXAMPLE_DERIVATIVE, EXAMPLE_LM_IDENT

if(nargin==1)
    y0 = 0;
else
    y0 = varargin{1};
end

if (size(U,2)>2 || size(U,2)<1)
    error('input U must have size 1 or 2');
elseif (size(U,2)==2)
    % first coloumn = y
    % second coloumn = u
    X = U(:,1);
    U = U(:,2);
    Y = X./(1+X.^2) + U.^3;
else % size(U,2)=1
    % step1
    X = y0;
    % steps
    for ii=1:length(U)
        Y(ii) = X(ii)/(1+X(ii).^2) + U(ii).^3;
        X(ii+1) = Y(ii);
    end

    X = X(1:end-1);
    X = X'; Y = Y';
end


function f = mckinnon ( x )

%*****************************************************************************80
%
%% MCKINNON computes the McKinnon function.
%
%  Discussion:
%
%    This function has a global minimizer:
%
%      X* = ( 0.0, -0.5 ), F(X*) = -0.25.
%
%    There are three parameters, TAU, THETA and PHI.
%
%    1 < TAU, then F is strictly convex.
%             and F has continuous first derivatives.
%    2 < TAU, then F has continuous second derivatives.
%    3 < TAU, then F has continuous third derivatives.
%
%    However, this function can cause the Nelder-Mead optimization
%    algorithm to "converge" to a point which is not the minimizer
%    of the function F.
%
%    Sample parameter values which cause problems for Nelder-Mead
%    include:
%
%      TAU = 1, THETA = 15, PHI =  10;
%      TAU = 2, THETA =  6, PHI =  60;
%      TAU = 3, THETA =  6, PHI = 400;
%
%    To get the bad behavior, we also assume the initial simplex has the form
%
%      X1 = (0,0),
%      X2 = (1,1),
%      X3 = (A,B),
%
%    where
%
%      A = (1+sqrt(33))/8 =  0.84307...
%      B = (1-sqrt(33))/8 = -0.59307...
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ken McKinnon,
%    Convergence of the Nelder-Mead simplex method to a nonstationary point,
%    SIAM Journal on Optimization,
%    Volume 9, Number 1, 1998, pages 148-158.
%
%  Parameters:
%
%    Input, real X(2), the argument of the function.
%
%    Output, real F, the value of the function at X.
%
  if ( length ( x ) ~= 2 )
    error ( 'Error: function expects a two dimensional input\n' );
  end

  tau = 2.0;
  theta = 6.0;
  phi = 60.0;

  if ( x(1) <= 0.0 )
    f = theta * phi * abs ( x(1) ).^tau + x(2) * ( 1.0 + x(2) );
  else
    f = theta       *       x(1).^tau   + x(2) * ( 1.0 + x(2) );
  end

  return
end


function [xi,w]=secondOrderPrismCubPoints()
%%SECONDORDERPRISMCUBPOINTS Generate second-order cubature points for
%  integration over a standard prism in 3D. This is a triangle that has
%  been extruded upward. The vertices are (1,-1,-1), (-1,-1,-1), (-1,1,-1),
%  (1,-1,1), (-1,-1,1), and (-1,1,1).
%
%INPUTS: None
%
%OUTPUTS: xi This is a 3XnumCubPoints set of points for the standard prism.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the standard prism (4).
%
%This function implements the points given in [1] (5 points).
%
%EXAMPLE:
%We compare a 2nd-order moment computed using these cubature points
%to one computed using monomialIntPrism. The results are the same within
%typical finite precision limits.
% [xi,w]=secondOrderPrismCubPoints();
% alpha=[2;0;0];
% theMoment=findMomentFromSamp(alpha,xi,w);
% intVal=monomialIntPrism(alpha);
% RelErr=(theMoment-intVal)/intVal
%
%REFERENCES:
%[1] F. D. Witherden and P. E. Vincent, "On the identification of symmetric
%    quadrature rules for finite element methods," Computer and Mathematics
%    with Applications, vol. 69, no. 10, pp. 1232-1241, May 2015.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

M=[-0.33333333333333333333333333333333333333,  -0.33333333333333333333333333333333333333,  -0.99999999999999985119303811076522004183,   0.66666666666666686507594918564641756458;
-0.33333333333333333333333333333333333333,  -0.33333333333333333333333333333333333333,   0.99999999999999985119303811076522004183,   0.66666666666666686507594918564641756458;
-0.74158162379719638007464124598498266439,   0.48316324759439276014928249196996532878,                                          0,   0.88888888888888875661603387623572162361;
 0.48316324759439276014928249196996532878,  -0.74158162379719638007464124598498266439,                                          0,   0.88888888888888875661603387623572162361;
-0.74158162379719638007464124598498266439,  -0.74158162379719638007464124598498266439,                                          0,   0.88888888888888875661603387623572162361];

w=M(:,4);
xi=M(:,1:3)';

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 variance = f_variance ( m, n )

%*****************************************************************************80
%
%% F_VARIANCE returns the variance of the F central PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the parameters of the PDF.
%    1 <= M,
%    1 <= N.
%    Note, however, that the variance is not defined unless 5 <= N.
%
%    Output, real VARIANCE, the variance of the PDF.
%
  if ( n < 5 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'F_VARIANCE - Fatal error!\n' );
    fprintf ( 1, '  The variance is not defined for N < 5.\n' );
    error ( 'F_VARIANCE - Fatal error!' );
  end

  variance = 2 * n * n * ( m + n - 2 ) / ( m * ( n - 2 )^2 * ( n - 4 ) );

  return
end