function omega_en_n = transport_rate(lat, Vn, Ve, h)
% transport_rate: calculates the transport rate in the navigation frame.
%
% INPUT
%	lat, 1x1 latitude (rad).
%	Vn, 1x1 North velocity (m/s).
%   Ve, 1x1 East velocity (m/s).
%   h, altitude (m).
%
% OUTPUT
%	omega_en_n, 3x3 skew-symmetric transport rate matrix (rad/s).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%   Paul D. Groves. Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems. Second Edition.
% Eq. 5.44, page 177.
%
% Version: 002
% Date:    2022/01/25
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

h = abs(h);

[RM,RN] = radius(lat);

om_en_n(1,1) =   Ve / (RN + h);              % North
om_en_n(2,1) = -(Vn / (RM + h));             % East
om_en_n(3,1) = -(Ve * tan(lat) / (RN + h));  % Down

omega_en_n = skewm(om_en_n);

end


function d = distancePointLine3d(point, line)
%DISTANCEPOINTLINE3D Euclidean distance between 3D point and line.
%
%   D = distancePointLine3d(POINT, LINE);
%   Returns the distance between point POINT and the line LINE, given as:
%   POINT : [x0 y0 z0]
%   LINE  : [x0 y0 z0 dx dy dz]
%   D     : (positive) scalar
%
%   See also:
%   lines3d, isPointOnLine3d, distancePointEdge3d, projPointOnLine3d,
%
%
%   References
%   http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html

%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 23/05/2005.
%

%   HISTORY
%   15/01/2007 unify size of input data
%   31/01/2007 typo in data formatting, and replace norm by vecnorm3d
%   12/12/2010 changed to bsxfun implementation - Sven Holcombe

% cf. Mathworld (distance point line 3d)  for formula
d = bsxfun(@rdivide, vectorNorm3d( ...
        crossProduct3d(line(:,4:6), bsxfun(@minus, line(:,1:3), point)) ), ...
        vectorNorm3d(line(:,4:6)));


function variance = hypergeometric_variance ( n, m, l )

%*****************************************************************************80
%
%% HYPERGEOMETRIC_VARIANCE returns the variance of the Hypergeometric PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of balls selected.
%    0 <= N <= L.
%
%    Input, integer M, the number of white balls in the population.
%    0 <= M <= L.
%
%    Input, integer L, the number of balls to select from.
%    0 <= L.
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = n * m * ( l - m ) * ( l - n ) / ( l * l * ( l - 1 ) );

  return
end


function p = perms_m(n, m)
%perms_m(n, m): return the mth permutation of the digits 1:n
% where m is from 0 to factorial(n) - 1
%
% The ordering is fairly arbitrary, but there is a bijection between m to
% the n! permutations (asking for same m twice gets same permutation), and
% perms_m(n, 0) is 1:n in order.
%
% Example:
%   p = perms(1:4);
%   P = zeros(size(p));
%   for i = 1:size(P,1)
%       P(i, :) = perms_m(4, i-1)';
%   end
%   all(all(sortrows(P) == sortrows(p)))
%
% See also: perms, randperm, uperms, signs_m, nchoosek_m, sample_no_repl

% Copyright 2010 Ged Ridgway
% http://www.mathworks.com/matlabcentral/fileexchange/authors/27434

n = ceil(abs(n)); m = round(abs(m));

M = factorial(n) - 1; % max m
if m < 0 || m > M
    error('perms_m:m_range', 'Require 0 <= m <= %d', M);
end

p = zeros(n,1);
d = 1:n;
F = factorial(n); % (F in loop goes from factorial(n-1) to factorial(0))
for f = 1:n
    F = F / (n + 1 - f); % (here rather than end of loop to avoid div-by-0)
    x = floor(m / F);
    p(f) = d(f + x);
    d(f + x) = d(f);
    m = rem(m, F);
end


function mask = maskSphere( d, r, show )
% Creates an 'image' of a n-dimensional hypersphere.
%
% Useful for testing visualization procedures.
%
% Create a d-dimensional matrix mask of dimensions [s x s x s x ... x s]
% where s=2r+1.  Each element mask(x1,x2...,xd) is 1 if (x1,...xd) falls
% inside the centered hypersphere of rad r.
%
% In 1d this corresponds to a vector of the form [.. 0 1 1 1 1 1 0 ..]
% In 2d this corresponds to an image of a white circle.
% In 3d and 4d, well try it and 'see' what it looks like.
%
% USAGE
%  mask = maskSphere( d, r, [show] )
%
% INPUTS
%  d         - dimension (any positive integer)
%  r         - sphere integer radius
%  show      - [] figure in which to display results
%
% OUTPUTS
%  mask      - [s x s x s x ... x s] hypersphere image
%
% EXAMPLE
%  mcircle = maskSphere( 2, 20, 1 );
%  msphere = maskSphere( 3, 10, 2 );
%  msphere = maskSphere( 4, 10, 3 );
%
% See also MASKCIRCLE, MONTAGE2
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<3 || isempty(show) ); show = 1; end

xs=cell(1,d);
for i=1:d; xs{i}=-r:r; end
if( d>1 ); [xs{:}] = ndgrid(xs{:}); else xs{1}=xs{1}'; end
mask=xs{1}.^2;
for i=2:d; mask=mask+xs{i}.^2; end
mask = double( mask < (r+1)^2 );

if( show )
  figure(show); clf;
  if( d<=2 )
    im( mask );
  elseif any(d==3:4)
    montage2( mask );
  else
    disp('no visualization available for d>4');
  end
end


function alpha_pq = moment_normalized ( n, x, y, p, q )

%*****************************************************************************80
%
%% MOMENT_NORMALIZED computes a normalized moment of a polygon.
%
%  Discussion:
%
%    Alpha(P,Q) = Integral ( x, y in polygon ) x^p y^q dx dy / Area ( polygon )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carsten Steger,
%    On the calculation of arbitrary moments of polygons,
%    Technical Report FGBV-96-05,
%    Forschungsgruppe Bildverstehen, Informatik IX,
%    Technische Universitaet Muenchen, October 1996.
%
%  Parameters:
%
%    Input, integer N, the number of vertices of the polygon.
%
%    Input, real X(N), Y(N), the vertex coordinates.
%
%    Input, integer P, Q, the indices of the moment.
%
%    Output, real ALPHA_PQ, the normalized moment Alpha(P,Q).
%
  nu_pq = moment ( n, x, y, p, q );
  nu_00 = moment ( n, x, y, 0, 0 );

  alpha_pq = nu_pq / nu_00;

  return
end


% MEDIAN Median value.
%    For vectors, MEDIAN(x) is the median value of the elements in x.
%    For matrices, MEDIAN(X) is a row vector containing the median value
%    of each column.  For N-D arrays, MEDIAN(X) is the median value of the
%    elements along the first non-singleton dimension of X.
%
%    MEDIAN(X,DIM) takes the median along the dimension DIM of X.
%
%    MEDIAN(...,NANFLAG) specifies how NaN (Not-A-Number) values
%    are treated. The default is 'includenan':
%
%    'includenan' - the median of a vector containing NaN values is also NaN.
%    'omitnan'    - the median of a vector containing NaN values is the
%                   median of all its non-NaN elements. If all elements
%                   are NaN, the result is NaN.
%
%    Example:
%        X = [1 2 4 4; 3 4 6 6; 5 6 8 8; 5 6 8 8]
%        median(X,1)
%        median(X,2)
%
%    Class support for input X:
%       float: double, single
%       integer: uint8, int8, uint16, int16, uint32, int32, uint64, int64
%
%    See also MEAN, STD, MIN, MAX, VAR, COV, MODE.
%
%    Reference page in Doc Center
%       doc median
%
%    Other functions named median
%
%       categorical/median    duration/median    timeseries/median
%       datetime/median       tall/median        ts/median
%       distributed/median
%


function sca=RLSsca(X,loc,p)
%Syntax: sca=RLSsca(X,loc,p)
%___________________________
%
% Calculates the Reweighted Least Squares (RLS) scale parameter
% of the columns of a matrix X.
%
% sca is the RLS estimated vector of scales.
% X is the matrix with the data sets.
% loc is the location vector. Its default value is the LMS location.
% p is the number of parameters. Its default value is 1.
%
% Reference:
% Rousseeuw PJ, Leroy AM (1987): Robust regression and outlier detection. Wiley.
%
%
% Alexandros Leontitsis
% Institute of Mathematics and Statistics
% University of Kent at Canterbury
% Canterbury
% Kent, CT2 7NF
% U.K.
%
% University e-mail: al10@ukc.ac.uk (until December 2002)
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% Sep 3, 2001.

if nargin<1 | isempty(X)==1
   error('Not enough input arguments.');
else
   % X must be 2-dimensional
   if ndims(X)>2
      error('Invalid data set.');
   end
   % If X is a row vector make it a column vector
   if size(X,1)==1
      X=X';
   end

   % n is the length of the data set
   n=size(X,1);
end

% For a single data point there is no need to proceed
if n==1
   sca=0;
else

   % Check loc
   if nargin<2 | isempty(loc)==1
      % If you don't give loc, it is supposed the LMS location
      loc=LMSloc(X);
   else
      % loc must be a scalar/vector
      if min(size(loc))>1
         error('loc must be a scalar/vector.');
      end
      % loc must have the length of the columns of X
      if length(loc)~=size(X,2);
         error('loc must have the length of the columns of X.');
      end
   end

   % Check p
   if nargin<3 | isempty(p)==1
      % If p is omitted give it the value of 1
      p=1;
   else
      % p must be a scalar
      if sum(size(p))>2
         error('p must be a scalar.');
      end
      % p must be an integrer greater than 1
      if round(p)-p~=0 | p<1
         error('p must be an integrer greater than or equal to 1.');
      end
   end

   % Calculate the scale parameter for every column
   for i=1:size(X,2)

      % The differences from the location
      r=X(:,i)-loc(i);

      % Estimate the preliminary scale parameter
      s=LMSsca(r,0,p);
      if s==0
         w=1:n;
      else
         w=find(abs(r)/s<=2.5);
      end
      sca(i)=sqrt(sum(r(w).^2)/(length(w)-p));
   end
end


function a = hankel ( n, x )

%*****************************************************************************80
%
%% HANKEL returns a HANKEL matrix.
%
%  Formula:
%
%    A(I,J) = X(I+J-1)
%
%  Example:
%
%    N = 5, X = ( 1, 2, 3, 4, 5, 6, 7, 8, 9 )
%
%    1  2  3  4  5
%    2  3  4  5  6
%    3  4  5  6  7
%    4  5  6  7  8
%    5  6  7  8  9
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is a Hankel matrix: constant along anti-diagonals.
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real X(2*N-1), the vector that defines A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for j = 1 : n
    a(1:n,j) = x(j:j+n-1);
  end

  return
end


function P = khatrirao(varargin)
%KHATRIRAO Khatri-Rao product of matrices.
%
%   KHATRIRAO(A,B) computes the Khatri-Rao product of matrices A and
%   B that have the same number of columns.  The result is the
%   column-wise Kronecker product
%   [KRON(A(:,1),B(:,1)) ... KRON(A(:,n),B(:,n))]
%
%   KHATRIRAO(A1,A2,...) computes the Khatri-Rao product of
%   multiple matrices that have the same number of columns.
%
%   KHATRIRAO(C) computes the Khatri-Rao product of
%   the matrices in cell array C.
%
%   KHATRIRAO(...,'r') computes the Khatri-Rao product in reverse
%   order.
%
%   Examples
%   A = rand(5,2); B = rand(3,2); C = rand(2,2);
%   khatrirao(A,B) %<-- Khatri-Rao of A and B
%   khatrirao(B,A,'r') %<-- same thing as above
%   khatrirao({C,B,A}) %<-- passing a cell array
%   khatrirao({A,B,C},'r') %<-- same as above
%
%   See also TENSOR, KTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) 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

%% Error checking on input and set matrix order
% Note that this next if/else check forces A to be a cell array.
if ischar(varargin{end}) && varargin{end} == 'r'
    if nargin == 2 && iscell(varargin{1})
        % Input is a single cell array
        A = varargin{1};
    else
        % Input is a sequence of matrices
        A = {varargin{1:end-1}};
    end
    matorder = length(A):-1:1;
else
    if nargin == 1 && iscell(varargin{1})
        % Input is a single cell array
        A = varargin{1};
    else
        % Input is a sequence of matrices
        A = varargin;
    end
    matorder = 1:length(A);
end

%% Error check on matrices and compute number of rows in result

% N = number of columns (must be the same for every input)
N = size(A{1},2);

% After loop, M = number of rows in the result
M = 1;

for i = matorder
    if ndims(A) ~= 2
        error('Each argument must be a matrix');
    end
    if (N ~= size(A{i},2))
        error('All matrices must have the same number of columns.')
    end
    M = M * size(A{i},1);
end

%% Computation

% Preallocate
P = zeros(M,N);

% Loop through all the columns
for n = 1:N
    % Loop through all the matrices
    ab = A{matorder(1)}(:,n);
    for i = matorder(2:end)
       % Compute outer product of nth columns
       ab = A{i}(:,n) * ab(:).';
    end
    % Fill nth column of P with reshaped result
    P(:,n) = ab(:);
end


function [rank,WT]=rankSetPartition(q,WT)
%%RANKSETPARTITION Return the set partition with the given rank in the
%           lexicographic ordering of all set parititions having length(n).
%           The partition is a length-n vector q that specifies which
%           partition each item belongs to. The order of the items in the
%           partitions is not important.
%
%INPUTS: q The nX1 set partition vector that should be ranked. Paritions
%          are numbered in increasing order starting from 1. That is, the
%          first occurence of partition numbered i in q is always before
%          the first occurences of partitions numbered j>1.
%       WT The is, optionally, the Williamson table. It is the table T in
%          [1]. The table is a function of length(q). If omitted, then the
%          table will be computed and returned so that futue calls to the
%          function can use
%
%OUTPUTS: rank The rank of the set partition in lexicographic order.
%              0<=rank<BellNumber(n).
%           WT The Williamson table for a length-n set partition. Passing
%              this to rankSetPartition on subsequent calls will speed up
%              the algorithm.
%
%This function implements the ranking algorithm of [1]. See
%getNextSetPartition for more information on set partitions.
%
%REFERENCES:
%[1] S. G. Williamson, "Ranking algorithms for lists of partitions," SIAM
%    Journal on Computing, vol. 5, no. 4, pp. 602-617, 1976.
%
%January 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=length(q);

if(nargin<3||isempty(WT))
    %Compute the Williamson triangle
    WT=zeros(n,n);
    WT(1,1:n)=1;
    for v=2:n
        for mu=1:(n-v+1)
            WT(v,mu)=mu*WT(v-1,mu)+WT(v-1,mu+1);
        end
    end
end

q=q-1;

rank=0;

m=cummax(q);

for t=0:(n-2)
    idx=n-t;

    rank=rank+q(idx)*WT(t+1,m(idx-1)+1);
end

rank=rank+1;

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 r = corr ( n, x, m )

%*****************************************************************************80
%
%% CORR computes the sample correlation of a signal sample.
%
%  Discussion:
%
%    The sample correlation is defined, for 0 <= i < N, as
%
%      R(i) = 1/N * sum ( 0 <= j <= N - 1 - i ) X(i+j) * X(j)
%
%    The sample correlation is an estimate of the correlation function.
%
%    It is usually the case that the signal X is assumed to
%    have zero mean.  Here, we compute the mean and adjust the
%    calculation accordingly:
%
%      R(i) = 1/N * sum ( 0 <= j <= N - 1 - i )
%        ( X(i+j) - Xbar ) * ( X(j) - Xbar )
%
%    Experience suggests that only the first 5 or 10 percent of
%    the lags are statistically reliable, so that one might choose
%    M = N / 20 or M = N / 10, for instance.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 June 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Sophocles Orfanidis,
%    Introduction to Signal Processing,
%    Prentice-Hall, 1995,
%    ISBN: 0-13-209172-0,
%    LC: TK5102.5.O246.
%
%  Parameters:
%
%    Input, integer N, the number of equally spaced signal
%    samples.
%
%    Input, real X(N), the signal samples.
%
%    Input, integer M, the maximum lag to consider.
%    0 <= M < N.
%
%    Output, real R(1:M+1), the sample correlations.
%
  r(1:m+1) = 0.0;

  xbar = sum ( x(1:n) ) / n;

  for i = 0 : m
    for j = 0 : n - i - 1
      r(i+1) = r(i+1) + ( x(i+j+1) - xbar ) * ( x(j+1) - xbar );
    end
  end

  r(1:m+1) = r(1:m+1) / n;

  return
end


function H = centeringMatrix(dim)

% CENTERINGMATRIX returns the centering matrix for the given dimensionality.
% FORMAT
% DESC returns the centering matrix for the given dimensionality.
% ARG dim : the dimensionality of the centering matrix.
% RETURN H : the centering matrix for the given dimensionality.
%
% COPYRIGHT : Neil D. Lawrence, 2008
%
% SEEALSO : eye

% NDLUTIL

H = -repmat(1./dim, dim, dim) + speye(dim);


function [ o, c, e ] = polynomial_mul ( m, o1, c1, e1, o2, c2, e2 )

%*****************************************************************************80
%
%% POLYNOMIAL_MUL multiplies two polynomials.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer O1, the "order" of polynomial 1.
%
%    Input, real C1(O1), the coefficients of polynomial 1.
%
%    Input, integer E1(O1), the indices of the exponents of polynomial 1.
%
%    Input, integer O2, the "order" of polynomial 2.
%
%    Input, real C2(O2), the coefficients of polynomial 2.
%
%    Input, integer E2(O2), the indices of the exponents of polynomial 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.
%
  o = o1 * o2;
  e = zeros ( o, 1 );
  c = zeros ( o, 1 );

  o = 0;
  for j = 1 : o2
    for i = 1 : o1
      o = o + 1;
      c(o) = c1(i) * c2(j);
      f1(1:m) = mono_unrank_grlex ( m, e1(i) );
      f2(1:m) = mono_unrank_grlex ( m, e2(j) );
      f(1:m) = f1(1:m) + f2(1:m);
      e(o) = mono_rank_grlex ( m, f );
    end
  end

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

  return
end


% 2-D Transform
% The 2-D Fourier transform is useful for processing 2-D signals and other 2-D data such as images.
% Create and plot 2-D data with repeated blocks.

P = peaks(20);
X = repmat(P,[5 10]);
imagesc(X)

% TODO : Compute the 2-D Fourier transform of the data.
% Shift the zero-frequency component to the center of the output, and
% plot the resulting 100-by-200 matrix, which is the same size as X.

% In the case of Radar signal processing. Convert the signal in MxN matrix, where M is the size of Range FFT samples and N is the size of Doppler FFT samples:
signal  = reshape(X, [100, 200]);

% Run the 2D FFT across both the dimensions.
signal_fft = fft2(signal, 100, 200);

% Shift zero-frequency terms to the center of the array
signal_fft = fftshift(signal_fft);

% Take the absolute value
signal_fft = abs(signal_fft);

% Here since it is a 2D output, it can be plotted as an image. Hence, we use the imagesc function
imagesc(signal_fft);


function [ x, w ] = lobatto_compute ( n )

%*****************************************************************************80
%
%% LOBATTO_COMPUTE computes a Lobatto quadrature rule.
%
%  Discussion:
%
%    The integral:
%
%      Integral ( -1 <= X <= 1 ) F(X) dX
%
%    The quadrature rule:
%
%      Sum ( 1 <= I <= N ) W(I) * F ( X(I) )
%
%    The quadrature rule will integrate exactly all polynomials up to
%    X^(2*N-3).
%
%    The Lobatto rule is distinguished by the fact that both endpoints
%    (-1 and 1) are always abscissas of the rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 April 2015
%
%  Author:
%
%    Original MATLAB code by Greg von Winckel.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Claudio Canuto, Yousuff Hussaini, Alfio Quarteroni, Thomas Zang,
%    Spectral Methods in Fluid Dynamics,
%    Springer, 1993,
%    ISNB13: 978-3540522058,
%    LC: QA377.S676.
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%    Daniel Zwillinger, editor,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996,
%    ISBN: 0-8493-2479-3.
%
%  Parameters:
%
%    Input, integer N, the order.
%
%    Output, real X(N), the abscissas.
%
%    Output, real W(N), the weights,
%
 if ( n == 1 )
    x(0) = -1.0;
    w(0) = 2.0;
    return
  end

  x = zeros ( n, 1 );
  w = zeros ( n, 1 );

  tolerance = 100.0 * eps;
%
%  Initial estimate for the abscissas is the Chebyshev-Gauss-Lobatto nodes.
%
  x(1:n,1) = cos ( pi * ( 0 : n - 1 ) / ( n - 1 ) )';
  xold(1:n,1) = 2.0;

  while ( tolerance < max ( abs ( x(1:n,1) - xold(1:n,1) ) ) )

    xold(1:n,1) = x(1:n,1);

    p(1:n,1) = 1.0;
    p(1:n,2) = x(1:n,1);

    for j = 2 : n-1
      p(1:n,j+1) = ( ( 2 * j - 1 ) * x(1:n,1) .* p(1:n,j)     ...
                   + (   - j + 1 ) *             p(1:n,j-1) ) ...
                   / (     j     );
    end

    x(1:n,1) = xold(1:n,1) - ( x(1:n,1) .* p(1:n,n) - p(1:n,n-1) ) ...
             ./ ( n * p(1:n,n) );
  end

  x(1:n) = r8vec_reverse ( n, x );

  w(1:n,1) = 2.0 ./ ( ( n - 1 ) * n * p(1:n,n).^2 );

  return
end


%GENDATS Generation of a simple classification problem of 2 Gaussian classes
%
%   A = GENDATS (N,K,D,LABTYPE)
%
% INPUT
%   N       Dataset size, or 2-element array of class sizes (default: [50 50]).
%   K       Dimensionality of the dataset to be generated (default: 2).
%   D       Distance between class means in the first dimension (default: 1).
%   LABTYPE Label type to generate, 'crisp' or 'soft' (default: 'crisp').
%
% OUTPUT
%   A       Dataset.
%
% DESCRIPTION
% Generation of a K-dimensional 2-class dataset A of N objects. Both classes
% are Gaussian distributed with identity matrix as covariance matrix. Their
% means are on a distance D. Class priors are P(1) = P(2) = 0.5.
%
% If N is a vector of sizes, exactly N(I) objects are generated for class I,
% I = 1,2.
%
% LABTYPE defines the desired label type: 'crisp' or 'soft'. In the latter
% case true posterior probabilities are set for the labels.
%
% SEE ALSO (PRTools Guide)
% DATASETS, PRDATASETS

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

% $Id: gendats.m,v 1.3 2009/01/27 13:01:42 duin Exp $

function A = gendats (N,k,d,labtype)

		if (nargin < 1),
		prwarning(2,'class sizes not specified, assuming [50 50]');
		N = [50 50];
	end
	if (nargin < 2),
		prwarning(2,'dimensionality not specified, assuming 2');
		k = 2;
	end
	if (nargin < 3),
		prwarning(2,'class mean distance not specified, assuming 1');
		d = 2;
	end
	if (nargin < 4),
		prwarning(2,'label type not specified, assuming "crisp"');
		labtype = 'crisp';
	end

	% Set equal priors and generate random class sizes according to these.
	p = [0.5 0.5]; N = genclass(N,p);

	% Unit covariance matrices, zero mean except for distance D in first dim.
	GA = eye(k); GB = eye(k);
	ma = zeros(1,k); mb = zeros(1,k); mb(1) = d;
	U = prdataset([ma;mb],[1 2]');
	U = setprior(U,p);

	% Create dataset.
	A = gendatgauss(N,U,cat(3,GA,GB),labtype);
	A = setname(A,'Simple Problem');

return


function area = quad_area2_2d ( q )

%*****************************************************************************80
%
%% QUAD_AREA2_2D computes the area of a quadrilateral in 2D.
%
%  Discussion:
%
%    A quadrilateral is a polygon defined by 4 vertices.
%
%    This algorithm computes the area of the related
%    Varignon parallelogram first.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 May 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real Q(2,4), the vertices, specified in
%    counter clockwise order.
%
%    Output, real AREA, the area of the quadrilateral.
%

%
%  Define a parallelogram by averaging consecutive vertices.
%
  p(1:2,1:3) = ( q(1:2,1:3) + q(1:2,2:4) ) / 2.0;
  p(1:2,  4) = ( q(1:2,  4) + q(1:2,1  ) ) / 2.0;
%
%  Compute the area.
%
  area = parallelogram_area_2d ( p );
%
%  The quadrilateral's area is twice that of the parallelogram.
%
  area = 2.0 * area;

  return
end


clear all, close all, clc
n = 256;
w = exp(-i*2*pi/n);

% Slow
for i=1:n
    for j=1:n
        DFT(i,j) = w^((i-1)*(j-1));
    end
end

% Fast
[I,J] = meshgrid(1:n,1:n);
DFT = w.^((I-1).*(J-1));
imagesc(real(DFT))


function [inv_map,bi_inv_map,logdet,invA] = invchol_or_lu(A)
% INVCHOL_OR_LU
% Does a Cholesky decomposition on A and returns logdet, inverse and
% two function handles that respectively map X to A\X and A\X/A.
%

if nargin==0
    test_this();
    return;
end

if isreal(A)

    R = chol(A); %R'*R = A
    inv_map = @(X) R\(R'\X);

    % inv(A)*X*inv(A)
    bi_inv_map = @(X) (inv_map(X)/R)/(R');

    if nargout>2
        logdet = 2*sum(log(diag(R)));
        if nargout>3
            invA = inv_map(eye(size(A)));
        end
    end
else
    [L,T,p] = lu(A,'vector');
    P = sparse(p,1:length(p),1);
                       % P*A = L*U
                       % L is lower triangular, with unit diagonal and unit determinant
                       % T is upper triangular, det(T) = prod(diag(T), may have negative values on diagonal
                       % P is a permuation matrix: P' = inv(P) and det(P) is +1 or -1
                       %

    inv_map = @(X) T\(L\(P*X));

    % inv(A)*X*inv(A)
    bi_inv_map = @(X) ((inv_map(X)/T)/L)*P;

    if nargout>2
        logdet = sum(log(diag(T)))-log(det(P));
        if nargout>3
            invA = T\(L\P);
        end
    end
end

function test_this()
dim = 3;
A = randn(dim)+sqrt(-1)*randn(dim);
[inv_map,bi_inv_map,logdet,iA] = invchol_or_lu(A);
[logdet,log(det(A))]
X = randn(dim,3);
Y1 = A\X,
Y2 = inv_map(X)

Z1 = (A\X)/A
Z2 = bi_inv_map(X)

iA,
inv(A)


function [ u, v ] = uv_spiral ( n, x, y, c )

%*****************************************************************************80
%
%% UV_SPIRAL computes a spiral velocity vector field.
%
%  Discussion:
%
%    Note that the continuous velocity field (U,V)(X,Y) that is discretely
%    sampled here satisfies the homogeneous continuity equation, that is,
%    it has zero divergence.  In other words:
%
%      dU/dX + dV/dY = 0.
%
%    This is by construction, since we have
%
%      U(X,Y) =  10 * d/dY ( PHI(X) * PHI(Y) )
%      V(X,Y) = -10 * d/dX ( PHI(X) * PHI(Y) )
%
%    which guarantees zero divergence.
%
%    The underlying function PHI is defined by
%
%      PHI(Z) = ( 1 - cos ( C * pi * Z ) ) * ( 1 - Z )^2
%
%    where C is a parameter.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), Y(N), the coordinates of the evaluation points.
%
%    Input, real C, a parameter, typically between 0 and 2 * PI.
%
%    Output, real U(N), V(N), the velocity components.
%
  u = zeros ( n, 1 );
  v = zeros ( n, 1 );

  u =   10.0 * ( 1.0 - cos ( c * pi * x ) ) ...
           .* ( 1.0 - x ).^2 ...
           .* ( ...
               c * pi * sin ( c * pi * y ) .* ( 1.0 - y ).^2 ...
             - ( 1.0 - cos ( c * pi * y ) ) ...
             .* 2.0 .* ( 1.0 - y ) ...
             );

  v = - 10.0 * ( 1.0 - cos ( c * pi * y ) ) ...
           .* ( 1.0 - y ).^2 ...
           .* ( ...
               c * pi * sin ( c * pi * x ) .* ( 1.0 - x ).^2 ...
             - ( 1.0 - cos ( c * pi * x ) ) ...
             .* 2.0 .* ( 1.0 - x ) ...
             );

  return
end


function [RM,RN] = radius(lat)
% radius: calculates meridian and normal radii of curvature.
%
% INPUT
%   lat, Nx1 latitude (rad).
%
% OUTPUT
%   RM, Nx1 meridian radius of curvature (North-South)(m).
%   RN, Nx1 normal radius of curvature (East-West) (m).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%	Titterton, D.H. and Weston, J.L. (2004). Strapdown
% Inertial Navigation Technology (2nd Ed.). Institution
% of Engineering and Technology, USA. Eq. 2.6 and 2.7.
%
% 	Groves, P. (2008). Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems. Artech House, UK.
% Eq. 2.65 and 2.66.
%
%  	R. Gonzalez, J. Giribet, and H. Patiño. An approach to
% benchmarking of loosely coupled low-cost navigation systems,
% Mathematical and Computer Modelling of Dynamical Systems, vol. 21,
% issue 3, pp. 272-287, 2015. Eq. 11.
%
% Version: 006
% Date:    2021/03/19
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

a = 6378137.0;                  % WGS84 Equatorial radius in meters
e = 0.0818191908425;            % WGS84 eccentricity

e2 = e^2;
den = 1 - e2 .* (sin(lat)).^2;

% Meridian radius of curvature: radius of curvature for North-South motion.
RM = a .* (1-e2) ./ (den).^(3/2);

% Normal radius of curvature: radius of curvature for East-West motion.
% AKA transverse radius.
RN = a ./ sqrt(den);

end


function diameter = polygon_diameter_2d ( n, v )

%*****************************************************************************80
%
%% POLYGON_DIAMETER_2D computes the diameter of a polygon in 2D.
%
%  Discussion:
%
%    The diameter of a polygon is the maximum distance between any
%    two points on the polygon.  It is guaranteed that this maximum
%    distance occurs between two vertices of the polygon.  It is
%    sufficient to check the distance between all pairs of vertices.
%    This is an N^2 algorithm.  There is an algorithm by Shamos which
%    can compute this quantity in order N time instead.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of vertices of the polygon.
%
%    Input, real V(2,N), the vertices.
%
%    Output, real DIAMETER, the diameter of the polygon.
%
  diameter = 0.0;

  for i = 1 : n

    for j = i + 1 : n
      diameter = max ( diameter, ...
        sqrt ( ( v(1,i) - v(1,j) ).^2 + ( v(2,i) - v(2,j) ).^2 ) );
    end

  end

  return
end


function [ w, t ] = circle_rule ( nt )

%*****************************************************************************80
%
%% CIRCLE_RULE computes a quadrature rule for the unit circle.
%
%  Discussion:
%
%    The unit circle is the region:
%
%      x * x + y * y = 1.
%
%    The integral I(f) is then approximated by
%
%      Q(f) = 2 * pi * sum ( 1 <= i <= NT ) W(i) * F ( cos(T(i)), sin(T(i)) ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NT, the number of angles to use.
%
%    Output, real W(NT), the weights for the rule.
%
%    Output, real T(NT), the angles for the rule.
%
  w(1:nt) = 1.0 / nt;
  for it = 1 : nt
    t(it) = 2.0 * pi * ( it - 1 ) / nt;
  end

  return
end


function [axis, theta] = rotation3dAxisAndAngle(mat)
%ROTATION3DAXISANDANGLE Determine axis and angle of a 3D rotation matrix.
%
%   [AXIS, ANGLE] = rotation3dAxisAndAngle(MAT)
%   Where MAT is a 4-by-4 matrix representing a rotation, computes the
%   rotation axis (containing the points that remain invariant under the
%   rotation), and the rotation angle around that axis.
%   AXIS has the format [DX DY DZ], constrained to unity, and ANGLE is the
%   rotation angle in radians.
%
%   Note: this method use eigen vector extraction. It would be more precise
%   to use quaternions, see:
%   http://www.mathworks.cn/matlabcentral/newsreader/view_thread/160945
%
%
%   Example
%     origin = [1 2 3];
%     direction = [4 5 6];
%     line = [origin direction];
%     angle = pi/3;
%     rot = createRotation3dLineAngle(line, angle);
%     [axis angle2] = rotation3dAxisAndAngle(rot);
%     angle2
%     angle2 =
%           1.0472
%
%   See also
%   transforms3d, vectors3d, angles3d, eulerAnglesToRotation3d
%

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

% extract the linear part of the rotation matrix
A = mat(1:3, 1:3);

% extract eigen values and eigen vectors
[V, D] = eig(A - eye(3));

% we need the eigen vector corresponding to eigenvalue==1
[dummy, ind] = min(abs(diag(D)-1)); %#ok<ASGLU>

% extract corresponding eigen vector
vector = V(:, ind)';

% compute rotation angle
t = [A(3,2)-A(2,3) , A(1,3)-A(3,1) , A(2,1)-A(1,2)];
theta = atan2(dot(t, vector), trace(A)-1);

% If angle is negative, invert both angle and vector direction
if theta<0
    theta  = -theta;
    vector = -vector;
end

% try to get a point on the line
% seems to work, but not sure about stability
[V, D] = eig(mat-eye(4)); %#ok<ASGLU>
origin = V(1:3,4)'/V(4, 4);

% create line corresponding to rotation axis
axis = [origin vector];


function k = pascal_to_i4 ( i, j )

%*****************************************************************************80
%
%% PASCAL_TO_I4 converts Pacal triangle coordinates to a linear index.
%
%  Discussion:
%
%    We describe the grid points in a Pascal triangle in two ways:
%
%    As a linear index K:
%
%                     1
%                   2   3
%                 4   5   6
%               7   8   9   10
%
%    As elements (I,J) of Pascal's triangle:
%
%                     0,0
%                  1,0   0,1
%               2,0   1,1    0,2
%            3,0   2,1   1,2    0,3
%
%  Example:
%
%     K  I  J
%
%     1  0  0
%     2  1  0
%     3  0  1
%     4  2  0
%     5  1  1
%     6  0  2
%     7  3  0
%     8  2  1
%     9  1  2
%    10  0  3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, J, the row and column indices.  I and J must
%    be nonnegative.
%
%    Output, integer K, the linear index of the (I,J) element.
%
  if ( i < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PASCAL_TO_I4 - Fatal error!\n' );
    fprintf ( 1, '  I < 0.\n' );
    fprintf ( 1, '  I = %d\n', i );
    error ( 'PASCAL_TO_I4 - Fatal error!' );
  elseif ( j < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PASCAL_TO_I4 - Fatal error!\n' );
    fprintf ( 1, '  J < 0.\n' );
    fprintf ( 1, '  J = %d\n', j );
    error ( 'PASCAL_TO_I4 - Fatal error!' );
  end

  d = i + j;

  k = ( d * ( d + 1 ) ) / 2 + j + 1;

  return
end


function integral = ball01_monomial_integral ( e )

%*****************************************************************************80
%
%% BALL01_MONOMIAL_INTEGRAL returns monomial integrals in the unit ball.
%
%  Discussion:
%
%    The integration region is
%
%      X^2 + Y^2 + Z^2 <= 1.
%
%    The monomial is F(X,Y,Z) = X^E(1) * Y^E(2) * Z^E(3).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gerald Folland,
%    How to Integrate a Polynomial Over a Sphere,
%    American Mathematical Monthly,
%    Volume 108, Number 5, May 2001, pages 446-448.
%
%  Parameters:
%
%    Input, integer E(3), the exponents of X, Y and Z in the
%    monomial.  Each exponent must be nonnegative.
%
%    Output, real INTEGRAL, the integral.
%
  if ( any ( e(1:3) < 0 ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BALL01_MONOMIAL_INTEGRAL - Fatal error!\n' );
    fprintf ( 1, '  All exponents must be nonnegative.\n' );
    error ( 'BALL01_MONOMIAL_INTEGRAL - Fatal error!' );
  end
%
%  Integrate over the surface.
%
  if ( all ( e(1:3) == 0 ) )

    integral = 2.0 * sqrt ( pi ^ 3 ) / gamma ( 1.5 );

  elseif ( any ( mod ( e(1:3), 2 ) == 1 ) )

    integral = 0.0;

  else

    integral = 2.0;

    for i = 1 : 3
      integral = integral * gamma ( 0.5 * ( e(i) + 1 ) );
    end

    integral = integral / gamma ( 0.5 * ( sum ( e(1:3) + 1 ) ) );

  end
%
%  The surface integral is now adjusted to give the volume integral.
%
  r = 1.0;
  s = sum ( e(1:3) ) + 3;

  integral = integral * r ^ s / s;

  return
end


%%
%   Author: Yash Bansod
%
% GitHub: <https://github.com/YashBansod>
%
% Demonstrates the calculation of Jacobian for a 3DOF 2D Planar Robot

%% Clear the environment and the command line
clear;
clc;
close all;

%% Calculation of Jacobian for 3DOF 2D Planar Robot
syms t1 t2 t3 L1 L2 L3
orgX = 0;
orgY = 0;

X(1)= orgX+ L1*cos(t1);
Y(1)= orgY+ L1*sin(t1);
X(2)= X(1)+ L2*cos(t1 + t2);
Y(2)= Y(1)+ L2*sin(t1 + t2);
X(3)= X(2)+ L3*cos(t1 + t2 + t3);
Y(3)= Y(2)+ L3*sin(t1 + t2 + t3);

Jacob = [diff(X(3),t1) , diff(X(3), t2) , diff(X(3), t3);
            diff(Y(3), t1), diff(Y(3), t2) , diff(Y(3), t3)]


function fx = p06_fx ( x )

%*****************************************************************************80
%
%% P06_FX evaluates exp ( x ) - 2 - 1 / ( 10 * x )^2 + 2 / ( 100 * x )^3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(*), the point at which F is to be evaluated.
%
%    Output, real FX(*), the value of the function at X.
%
  fx = exp ( x ) - 2.0 - 1.0 ./ ( 10.0 * x ).^2 + 2.0 ./ ( 100.0 * x ).^3;

  return
end


%% RATE OF CONVERGENCE OF LINEAR ELEMENT FOR POISSON EQUATION
%
% This example is to show the rate of convergence of linear finite element
% approximation of the Poisson equation on the unit cube with the
% following boundary conditions:
%
% - Non-empty Dirichlet boundary condition.
% - Pure Neumann boundary condition.
% - Robin boundary condition.
%
% The basis, data structure and numerical test is summarized in <a
% href="matlab:ifem Poisson3femrate">Poisson3femrate</a>.
%
% See also Poisson3P2femrate, Poissonfemrate
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

clear variables

%% Setting
[node,elem] = cubemesh([0,1,0,1,0,1],0.5);
mesh = struct('node',node,'elem',elem);
option.L0 = 1;
option.maxIt = 4;
option.elemType = 'P1';
option.printlevel = 1;
option.plotflag = 1;

%% Non-empty Dirichlet boundary condition.
fprintf('Mixed boundary conditions. \n');
pde = sincosdata3;
mesh.bdFlag = setboundary3(node,elem,'Dirichlet','~(x==0)','Neumann','x==0');
% bdFlag = setboundary3(node,elem,'Dirichlet');
femPoisson3(mesh,pde,option);

%% Pure Neumann boundary condition.
fprintf('Pure Neumann boundary condition. \n');
option.plotflag = 0;
pde = sincosdata3Neumann;
mesh.bdFlag = setboundary3(node,elem,'Neumann');
femPoisson3(mesh,pde,option);

%% Robin boundary condition.
fprintf('Robin boundary condition. \n');
pde = sincosRobindata3;
mesh.bdFlag = setboundary3(node,elem,'Robin');
femPoisson3(mesh,pde,option);

%% Conclusion
%
% The optimal rate of convergence of the H1-norm (1st order) and L2-norm
% (2nd order) is observed. The 2nd order convergent rate between two
% discrete functions ||DuI-Duh|| is known as superconvergence.
%
% MGCG converges uniformly in all cases.
%
% Note that for Neumann and Robin boundary condition, the order of the error in the
% maximum norm is not close to 2. Neumann problem is around 1.5 and Robin
% is near 1.3.


%% Optimizing the operation of a Hydroelectric Dam
%
% This demo maximizes the revenue generated from a hydroelectric dam using
% Quadratic Programming.  We compute the optimal flow rate to the turbine
% as well as the optimal spill flow rate.
%
% This problem is constrained such that:
%
%  1)  0 < Turbine Flow < 25,000 CFS
%  2)  0 < Spill Flow
%  3)  Max change in Turbine and Spill Flow < 500 CFS
%  4)  Combined Turbine and Spill Flow > 1000 CFS
%  5)  50000 < Reservoir Storage < 100000 Acre-Feet
%  6)  The final reservoir level must equal the initial level (90000 AF)
%
% Two solvers are used to solve this problem: FMINCON and QUADPROG
% FMINCON is used first as it takes less time to set-up the problem for
% ths solver.  Supplying the Gradient and Hessian to the FMINCON algorithm
% significantly speeds up the solution of the problem.
% QUADPROG ionterior-point-convex is used third to provide a faster
% algorithm for solving this type of problem.  This solver is also better
% suited for handling a larger version of the same problem.
%
% Copyright (c) 2012, MathWorks, Inc.

%% 1 - Load data
% Two variables are loaded from data set:
%   inFlow - flow rate into the reservoir measured hourly (CFS)
%   price - price of electricity measured hourly ($/MWh)
load('FlowAndPriceData.mat');

%% 2 - Define Starting Point and Constants
% We wish to find the vector x of optimal decision variables.
% For this problem:
% x = [turbineFlow(1) ... turbineFlow(N) spillFlow(1) ... spillFlow(N)]

% Our initial starting point is turbineFlow = inFlow and spillFlow = 0
x0 = [inFlow; zeros(size(inFlow))];

stor0 = 90000; % initial vol. of water stored in the reservoir (Acre-Feet)

k1 = 0.00003; % K-factor coefficient
k2 = 9; % K-factor offset

MW2kW = 1000; % MW to kW
C2A = 1.98347/24; % Convert from CFS to AF/HR

C = [C2A k1 k2 MW2kW]; % Vector of constants

N = length(inFlow);

%% 3 - Define Linear Inequality Constraints and Bounds
DefineConstraints;

%% 4 - Objective function
% Use symbolic math for creating the objective function and then convert
% into a MATLAB function that can be evaluated.
%% 4.1 - Derive Objective Function using Symbolic Math
% First define the decision variables as X, which is a vector consisting
% of [TurbineFlow, SpillFlow] and all the parameters as symbolic objects
X = sym('x',[2*N,1]);   % turbine flow and spill flow
F = sym('F',[N,1]);     % flow into the reservoir
P = sym('P',[N,1]);     % price
s0 = sym('s0');         % initial storage
c = sym({'c1','c2','c3','c4'}.','r');   % constants

%%
% Total out flow equation
TotFlow = X(1:N)+X(N+1:end);

%%
% Storage Equations
S = cell(N,1);
S{1} = s0 + c(1)*(F(1)-TotFlow(1));

for ii = 2:N
    S{ii} = S{ii-1} + c(1)*(F(ii)-TotFlow(ii));
end

%%
% K factor equation
k = c(2)*([s0; S(1:end-1)]+ S)/2+c(3);

% MWh equation
MWh = k.*X(1:N)/c(4);

%%
% Revenue equation
R = P.*MWh;

%%
% Total Revenue (Minus sign changes from maximization to minimization)
totR = -sum(R);

%% 4.2 - Create Function File that can be Evaluated in MATLAB
% Create a function that when evaluated returns the value at a point

% Perform substitutions so that X is the only remaining variable
totR = subs(totR,[c;s0;P;F],[C';stor0;price;inFlow]);

% Generate a MATLAB file from the equation
matlabFunction(totR,'vars',{X},'file','objFcn');

%% 5 - Optimize with FMINCON
options = optimset('MaxFunEvals',Inf,'MaxIter',5000,...
    'Algorithm','interior-point','Display','iter');

tic
[x1, fval1] = fmincon(@objFcn,x0,A,b,Aeq,beq,LB,UB,[],options);
toc

% Visualize Results
plotResults(price, x1, N);

%% 6 - Optimize with FMINCON with supplied Gradient and Hessian
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Let's see if we can speed up the optimization by supplying the gradient
% and Hessian.  Passing these to the algorithm allows it to make more
% accurate decisions because it no longer has to approximate the gradient
% and hessian (approximation also takes longer as it requires many
% objective function evaluations).

%% 6.1 - Gradient
% Use the GRADIENT function from Symbolic Math to calculate the gradient
gradObj = gradient(totR,X);

% Create a function that when evaluated returns the gradient at a point
matlabFunction(gradObj,'vars',{X},'file','grad');

% The optimization routine expects two return arguments for a user-supplied
% gradient.  The first argument is the value of the objective function and
% the second argument is the gradient.  The DEAL function is used to return
% multiple output arguments through a function handle.
ValAndGrad = @(x) deal(objFcn(x),grad(x));

%% 6.2 - Hessian
% Use the HESSIAN function from Symbolic Math to calculate the Hessian
hessObj = hessian(totR,X);

% Create a function that when evaluated returns the Hessian at a point
matlabFunction(hessObj,'vars',{X},'file','Hess');

% The Hessian function has two input arguments (the current point and the
% Lagrange Multipliers) and returns the value of the Hessian at the current
% point.
hfcn = @(x,lambda) Hess(x);

%% 6.3 - Optimize
options = optimset('Disp','iter','Algorithm','interior-point',...
                   'MaxFunEvals',500,'MaxIter',5000,...
                   'GradObj','on','Hessian','user-supplied',...
                   'HessFcn',hfcn);

tic
[x2,fval2] = fmincon(ValAndGrad,x0,A,b,Aeq,beq,LB,UB,[],options);
toc

% Visualize Results
plotResults(price, x2, N);

%% 7 - Optimize using the QUADPROG Solver
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% QUADPROG minimizes functions of the type (1/2) * x' * H * x  +  f' * x
% First perform some substitutions into the revenue equation
Rsub = subs(R,[c;s0;P;F],[C';stor0;price;inFlow]);

% Total revenue (minus sign to flip maximization to minimization)
Rtot = -sum(Rsub);

%% 7.2 - Calculate and evaluate the linear component
fsym = gradient(Rtot,X);
f = double(subs(fsym,X,zeros(size(X))));

%% 7.1 - Calculate and evaluate the quadratic matrix
H = 0.5.*double(hessian(Rtot,X));

%% 7.3 - Optimize
qpoptions = optimset('Algorithm','interior-point-convex','Disp','iter');

tic
[x3,fval3] = quadprog(H,f,A,b,Aeq,beq,LB,UB,[],qpoptions);
toc

% Visualize Results
plotResults(price, x3, N);


function TDD = divdiff(X, Y)
%
% TDD = divdiff(X, Y)
%
% DIVDIFF
%   Newton's Method for Divided Differences.
%
%   The following formula is solved:
%       Pn(x) = f(x0) + f[x0,x1](x-x0) + f[x0,x1,x2](x-x0)(x-x1) + ...
%           + f[x0,x1,..,xn](x-x0)(x-x1)..(x-x[n-1])
%       where f[x0,x1] = (f(x1-f(x0))/(x1-x0)
%             f[x0,x1,..,xn] = (f[x1,..,xn]-f[x0,..,x_[n-1]])/(xn-x0)
%
% NOTE: f^(n+1)(csi)/(n+1)! aprox. = f[x0,x1,..,xn,x_[n+1]]
%
% Input::
%	X = [ x0 x1 .. xn ] - object vector
%	Y = [ y0 y1 .. yn ] - image vector
%
% Output:
%   TDD - table of divided differences
%
% Example:
%   TDD = divdiff( [ 1.35 1.37 1.40 1.45 ], [ .1303 .1367 .1461 .1614 ])
%
% Author:	Tashi Ravach
% Version:	1.0
% Date:     22/05/2006
%

    if nargin ~= 2
        error('divdiff: invalid input parameters');
    end

    [ p, m ] = size(X); % m points, polynomial order <= m-1
    if p ~= 1 || p ~=size(Y, 1) || m ~= size(Y, 2)
        error('divdiff: input vectors must have the same dimension');
    end

    TDD = zeros(m, m);
    TDD(:, 1) = Y';
    for j = 2 : m
        for i = 1 : (m - j + 1)
            TDD(i,j) = (TDD(i + 1, j - 1) - TDD(i, j - 1)) / (X(i + j - 1) - X(i));
        end
    end

end


function [detjacobian,invjacobian]=Jacobian(nnel,dhdr,dhds,xcoord,ycoord)

%------------------------------------------------------------------------
%  Purpose:
%     determine the Jacobian for two-dimensional mapping
%
%  Synopsis:
%     [detjacobian,invjacobian]=Jacobian(nnel,dhdr,dhds,xcoord,ycoord)
%
%  Variable Description:
%     jacobian - Jacobian for one-dimension
%     nnel - number of nodes per element
%     dhdr - derivative of shape functions w.r.t. natural coordinate r
%     dhds - derivative of shape functions w.r.t. natural coordinate s
%     xcoord - x axis coordinate values of nodes
%     ycoord - y axis coordinate values of nodes
%------------------------------------------------------------------------

 jacobian=zeros(2,2);

 for i=1:nnel
 jacobian(1,1) = jacobian(1,1)+dhdr(i)*xcoord(i);
 jacobian(1,2) = jacobian(1,2)+dhdr(i)*ycoord(i);
 jacobian(2,1) = jacobian(2,1)+dhds(i)*xcoord(i);
 jacobian(2,2) = jacobian(2,2)+dhds(i)*ycoord(i);
 end

 detjacobian = det(jacobian) ;  % Determinant of Jacobian matrix
 invjacobian = inv(jacobian) ;  % Inverse of Jacobian matrix


function KM = computeKernelMatrix(P1, P2, kType, kParam)
%COMPUTEKERNELMATRIX computes the kernel matrix between two sets of patterns
%
%   KM = computeKernelMatrix(P1, P2, kType, kParam) computes kernel matrix
%   KM between sets of patterns P1 and P2. kType is the type of the kernel
%   function (Gauss, Linear, Quickrbf, Poly, or Sigmoid). KPARAM is the
%   kernel parameter, for instance the width of the Gaussian kernel.
%
% Function inputs:
%   patterns1, patterns2   - Two matrixes of patterns (train and test, or train and train)
%   kernelType             - Kernel function choice. Can be  Gauss, Linear, Quickrbf, Poly, or Sigmoid.
%   kernelMatrix           - The kernel matrix
%
%   This file is part of ORCA: https://github.com/ayrna/orca
%   Original authors: Pedro Antonio Gutiérrez, María Pérez Ortiz, Javier Sánchez Monedero
%   Citation: If you use this code, please cite the associated paper http://www.uco.es/grupos/ayrna/orreview
%   Copyright:
%       This software is released under the The GNU General Public License v3.0 licence
%       available at http://www.gnu.org/licenses/gpl-3.0.html
%
Nf1             = size(P1, 2);
Nf2             = size(P2, 2);
KM   = zeros(Nf1, Nf2);

switch upper(kType)
    case {'GAUSS','GAUSSIAN','RBF'}

        for i = 1:Nf2
            KM(:,i)  = exp(-kParam*sum((P1-P2(:,i)*ones(1,Nf1)).^2)');

        end

    case {'QUICKRBF'}
        d = pdistalt(P1,P2,'euclidean');
        KM = exp(-kParam*d.^2);

    case {'POLYNOMIAL', 'POLY', 'LINEAR'}
        if strcmpi(kType, 'LINEAR')
            kParam = 1;
            bias = 0;
        else
            bias = 1;
        end
        KM = ((P1' * P2 + bias)/size(P1,1)).^kParam;

    case 'SIGMOID'

        if (length(kParam) ~= 2)
            error('This kernel needs two parameters to operate!')
        end

        KM   = zeros(Nf1, Nf2);
        for i = 1:Nf2
            KM(:,i)  = tanh(P1'*P2(:,i)*kParam(1)+kParam(2));
        end

    otherwise
        error('Unknown kernel. Can be Gauss, Linear, Quickrbf, Poly, or Sigmoid.')

end


function X = lyap(A, B, C)
%LYAP  Solve continuous-time Lyapunov equations.
%
%   X = LYAP(A,C) solves the special form of the Lyapunov matrix
%   equation:
%
%           A*X + X*A' = -C
%
%   X = LYAP(A,B,C) solves the general form of the Lyapunov matrix
%   equation (also called Sylvester equation):
%
%           A*X + X*B = -C
%
%   See also  DLYAP.

%	S.N. Bangert 1-10-86
%	Copyright (c) 1986-98 by The MathWorks, Inc.
%	$Revision: 1.4 $  $Date: 1997/12/01 22:11:32 $
%	Last revised JNL 3-24-88, AFP 9-3-95

ni = nargin;

if ni==2,
   C = B;
   B = A';
end

[ma,na] = size(A);
[mb,nb] = size(B);
[mc,nc] = size(C);

% A and B must be square and C must have the rows of A and columns of B
if (ma ~= na) | (mb ~= nb) | (mc ~= ma) | (nc ~= mb)
   error('Dimensions do not agree.')
elseif ma==0,
   X = [];
   return
end

% Perform schur decomposition on A (and convert to complex form)
[ua,ta] = schur(A);
[ua,ta] = rsf2csf(ua,ta);
if ni==2,
   % Schur decomposition of A' can be calculated from that of A.
   j = ma:-1:1;
   ub = ua(:,j);
   tb = ta(j,j)';
else
   % Perform schur decomposition on B (and convert to complex form)
   [ub,tb] = schur(B);
   [ub,tb] = rsf2csf(ub,tb);
end

% Check all combinations of ta(i,i)+tb(j,j) for zero
p1 = diag(ta).'; % Use .' instead of ' in case A and B are not real
p1 = p1(ones(mb,1),:);
p2 = diag(tb);
p2 = p2(:,ones(ma,1));
sum = abs(p1) + abs(p2);
if any(any(sum == 0)) | any(any(abs(p1 + p2) < 1000*eps*sum))
   error('Solution does not exist or is not unique.');
end

% Transform C
ucu = -ua'*C*ub;

% Solve for first column of transformed solution
y = zeros(ma,mb);
ema = eye(ma);
y(:,1) = (ta+ema*tb(1,1))\ucu(:,1);

% Solve for remaining columns of transformed solution
for k=2:mb,
   km1 = 1:(k-1);
   y(:,k) = (ta+ema*tb(k,k))\(ucu(:,k)-y(:,km1)*tb(km1,k));
end

% Find untransformed solution
X = ua*y*ub';

% Ignore complex part if real inputs (better be small)
if isreal(A) & isreal(B) & isreal(C),
   X = real(X);
end

% Force X to be symmetric if ni==2 and C is symmetric
if (ni==2) & isequal(C,C'),
   X = (X+X')/2;
end

% end lyap


function DeriveDynamicsDP(P)

%FUNCTION:
%   This function is used to derive the analytic equations of motion for a
%   forced double pendulum. This also does the kimenatics and energy
%   calculations.

%State Naming Conventions:
% th = absolute angle of the first link measured wrt the positive horizontal axis (i)
% phi = absolute angle of the second link measured wrt the positive horizontal axis (i)
% Dth = first time derivative of th
% Dphi = first time derivative of phi

%NOTE: It might be faster to use the jacobian command to find the symbolic
%coeffieints of the mass matrix and then invert the system, rather than
%symbolically inverting the matrix.
%

% Define the angles and derivatives
    th = sym('th');
    Dth = sym('Dth');
    DDth = sym('DDth');

    phi = sym('phi');
    Dphi = sym('Dphi');
    DDphi = sym('DDphi');

% Define the scalar parameters
    g = sym('g');   %gravity
    L = sym('L');    %Lenght of each link
    m1 = sym('m1');    %mass of the link
    m2 = sym('m2');    %mass of the link

% Known Vectors
    F1_x = sym('F1_x');
    F1_y = sym('F1_y');
    F1 = [F1_x;F1_y];   %External force at CoM of first link

    F2_x = sym('F2_x');
    F2_y = sym('F2_y');
    F2 = [F2_x;F2_y];   %External force at CoM of 2nd link

    M1 = sym('M1');   %Input torque on link on
    M2 = sym('M2');   %Input torque between links 1 and 2

% UNIT VECTORS:
    i = [1;0];
    j = [0;1];

    a1 = cos(th)*i + sin(th)*j;   %The direction along the first link
    b1 = -sin(th)*i + cos(th)*j;
    Da1 = Dth*b1;
    Db1 = -Dth*a1;
    DDa1 = DDth*b1 + Dth*Db1;
    DDb1 = -DDth*a1 -Dth*Da1;

    a2 = cos(phi)*i + sin(phi)*j;   %The direction along the second link
    b2 = -sin(phi)*i + cos(phi)*j;
    Da2 = Dphi*b2;
    Db2 = -Dphi*a2;
    DDa2 = DDphi*b2 + Dphi*Db2;
    DDb2 = -DDphi*a2 -Dphi*Da2;

%Weights
    W1 = -m1*g*j;   %weight of the first link
    W2 = -m2*g*j;   %weight of the second link

%Kinematic vectors
    r_P1_O = L*a1;    %Position of the point mass 1
    v_P1_O = L*Da1;
    a_P1_O = L*DDa1;

    r_P2_O = L*a1 + L*a2;    %Position of point mass 2
    v_P2_O = L*Da1 + L*Da2;
    a_P2_O = L*DDa1 + L*DDa2;

    r_P2_P1 = L*a2;   %Relative position of the tip with respect to the mid joint

%Momentum balance for both links about the origin
    %       = Moment from weight 1 +  moment from weight 2 + Control torque 1
    Sum_M_O = Cross(r_P1_O,F1+W1) + Cross(r_P2_O,F2+W2) + M1;
    %        = translation mass 1  + translation mass 2
    Sum_DH_O = Cross(r_P1_O,m1*a_P1_O) + Cross(r_P2_O,m2*a_P2_O);

%Momentum balance for second link about central pivot
    %         moment from weight 2  +  control moment
    Sum_M_P1 = Cross(r_P2_P1,F2+W2) + M2;
    %         translation mass 2 +
    Sum_DH_P1 = Cross(r_P2_P1,m2*a_P2_O);

%Simplify
    AMB_1 = simplify(Sum_M_O - Sum_DH_O);
    AMB_2 = simplify(Sum_M_P1 - Sum_DH_P1);

% Solve the system of equations:
    Soln = solve( AMB_1, ...    %AMB for system
           AMB_2, ...    %AMB for second link
            DDth,...                      %acceleration of first joint
            DDphi  );                    %acceleration of second joint

    Soln.DDth = simplify(Soln.DDth);   %Simplify the result
    Soln.DDphi = simplify(Soln.DDphi);   %Simplify the result

%Write the results to a file  --  these are scripts!
if P.misc.CppFlag
    WriteDynamicsFile_Cpp    %Call a script that write the dynamics to a file
else
    WriteDynamicsFile_Matlab;   %Call a script that write the dynamics to a file
end
WriteKinematicsFile_Matlab;   %Call a script that writes the equations for kinetics

end


function [Moda , ncont] = iModa(x)

% A matlab script to find the mode of vector x.
% There are two options at the end so that it is possible to perform a checksum
% on data and plot the vector of frequencies, just uncomment the right lines!
%
% Isaac M. M. INOGS, Trieste(Italy). January 10, 2005 @01h46:44
% Developed under Matlab(TM) version 7.0.0.19901 (R14)
% Modified on January 13, 2005 @14h49:50
% in order to include suggestion from Jos jnvdg@arcor.de (thanks, Jos)
% and add some robustness regarding NANs (Not-A-Number)

x = x(:);
Xvalue = unique(x(~isnan(x)));             % find elements in x with no repetitions (sorted output)

Xvalue = Xvalue(:);        % elements in vector
Xcont = histc(x,Xvalue);
Xcont = Xcont(:);          % frequency of each element

[maxCont imaxCont] = max(Xcont);
ncont = maxCont;                     % frequency of mode
Moda = Xvalue(imaxCont);             % the mode

%%%%%% optional 1 - plot of frequencies for each element

 plot(Xvalue,Xcont);
 xlabel('values')
 ylabel('frequencies')

%%%%%% optional 2 - checksum

% disp(strcat('sum of frequencies :',num2str(sum(Xcont))));
% disp(strcat('lenght of vector :',num2str(length(x))));
% disp(strcat('Nans, if any :',num2str(sum(isnan(x)))));
% chcksm = (sum(Xcont)+sum(isnan(x))) == length(x);
% disp(chcksm)


function [xi,w]=firstOrderTriangleCubPoints()
%%FIRSTORDERTRIANGLECUBPOINTS Obtain first-order cubature points for
%   integration over a triangle in 2D. The points and weights are for the
%   triangle with vertices (1,0), (0,1), (0,0), but can be transformed to
%   any triangle using transformSimplexTriPoints.
%
%INPUTS: None
%
%OUTPUTS: xi A 2XnumCubPoints set of points for the standard triangle.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the triangle (1/2).
%
%This function implements the points given in [1] (3 points).
%
%EXAMPLE:
%Given the vertices of the simplex, we compare a first-order moment
%computed using these cubature points to one computed using
%monomialIntSimplex. The results are the same within typical finite
%precision limits.
% [xi,w]=firstOrderTriangleCubPoints();
% alpha=[1;0];
% theMoment=findMomentFromSamp(alpha,xi,w)
% intVal=monomialIntSimplex(alpha)
%
%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, 2];
w=M(:,3);
xi=M(:,1:2)';
%Transform the points to the standard triangle.
v1=[-1,-1, 1;
    -1, 1,-1];
v2=[1,0,0;
    0,1,0];
[A,d]=affineTransBetweenTriangles(v1,v2);
xi=bsxfun(@plus,A*xi,d);
w=w/4;
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.


%% Structured grid
% This is a sample code showing how to use ConstructProjector3D().
% This example is going to setup a transformation matrix based on forth
% order polynomials in 3D.
% Pay attention that no data is needed to construct the interpolator. Only
% the coordinates of the points on source and destination grid.
clear;clc;
%% Initializing Part I
disp('- Initializing')
xMin=0;
xMax=2*pi;
yMin=0;
yMax=2*pi;
zMin=0;
zMax=2*pi;

nx=20;
ny=20;
nz=20;

dx=(xMax-xMin)/(nx-1);
dy=(yMax-yMin)/(ny-1);
dz=(zMax-zMin)/(nz-1);

nPoly=2; % We gonna fit a fourth order polynomial,
         % i.e. sum_{n,m,t=0}^{n,m,t=nPoly} x^n*y^m*z^t
nInterp=27:37;

%% Turning off the warnings
% Depending on the parameter that you choose the matrices might be nearly
% singular. In this example, despite being nearly singular, they are not
% affecting the final interpolation. Therefore, I set the warning to off.
% But generally do not turn it off and if you got this warning check if it
% is going to affect your results or not.
% disp('- Turning the warnings off')
% warning('OFF','MATLAB:nearlySingularMatrix');

%% Generating the Source Grid
disp('- Generating the source grid.')
[xn,yn,zn]=meshgrid(linspace(xMin,xMax,nx),linspace(yMin,yMax,ny),linspace(zMin,zMax,nz));

%% Finding the cell centers
disp('- Generating the destination grid')
xc=xn(1:ny-1,1:nx-1,1:nz-1)+dx/2;
yc=yn(1:ny-1,1:nx-1,1:nz-1)+dy/2;
zc=zn(1:ny-1,1:nx-1,1:nz-1)+dz/2;

% figure
% plot3(xn(:),yn(:),zn(:),'k.','MarkerSize',20);
% hold on
% plot3(xc(:),yc(:),zc(:),'b.','MarkerSize',20);
% axis tight;
% axis square;
% title('Source Grid, black dots, & the destination grid, blue dots.', ...
%       'FontName','Arial','FontSize',12,'FontWeight','Bold');

%% Generarting some Data
disp('- Generating some data and interpolating')
F1=@(x,y,z) (sqrt(x.^2+y.^2+z.^2));
Vn=F1(xn,yn,zn);
Vc_Analytic=F1(xc,yc,zc);

%% Generating the interpolant
disp('- Generating the interpolant and testing nInterp')
RMSE=zeros(size(nInterp));
for i=1:numel(nInterp)
  disp(['-- nInterp= ' num2str(nInterp(i))])
  P=ConstructProjector3D(xn(:),yn(:),zn(:),xc(:),yc(:),zc(:),nPoly,nInterp(i));
  % Interpolating
  Vc_interp=P*Vn(:);

  % Calculating the RMSE
  RMSE(i)=sqrt(mean( (Vc_interp-Vc_Analytic(:)).^2 ));
  disp(['   RMSE= ' num2str(RMSE(i))])
end
%% plotting

figure
plot(nInterp,RMSE,'b.-')
xlabel('nInterp');
ylabel('RMSE');
axis tight

disp('All done!')


function value = torus_volume_3d ( r1, r2 )

%*****************************************************************************80
%
%% TORUS_VOLUME_3D returns the volume of a torus in 3D.
%
%  Integration region:
%
%    Points (X,Y,Z) such that:
%
%      ( SQRT ( X**2 + Y**2 ) - R1 )**2 + Z**2 = R2**2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R1, R2, the two radii that define the torus.
%
%    Output, real TORUS_VOLUME_3D, the volume of the torus.
%
  value = 2.0 * pi * pi * r1 * r2 * r2;

  return
end


function theta = circle3dPosition(point, circle)
%CIRCLE3DPOSITION Return the angular position of a point on a 3D circle
%
%   POS = circle3dPosition(POINT, CIRCLE)
%   Returns angular position of point on the circle, in degrees, between 0
%   and 360.
%   with POINT: [xp yp zp]
%   and CIRCLE: [X0 Y0 Z0 R THETA PHI] or [X0 Y0 Z0 R THETA PHI PSI]
%   (THETA being the colatitude, and PHI the azimut)
%
%   See also:
%   circles3d, circle3dOrigin, circle3dPoint
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 21/02/2005
%

%   HISTORY
%   27/06/2007: change 3D angle convention
%   2011-06-21 use degrees for angles

% get center and radius
xc = circle(:,1);
yc = circle(:,2);
zc = circle(:,3);

% get angle of normal
theta   = circle(:,5);
phi     = circle(:,6);

% find origin of the circle
ori     = circle3dOrigin(circle);

% normal vector of the supporting plane (cartesian coords)
vn      = sph2cart2d([theta phi]);

% create plane containing the circle
plane   = createPlane([xc yc zc], vn);

% find position of point on the circle plane
pp0     = planePosition(ori,    plane);
pp      = planePosition(point,  plane);

% compute angles in the planes
theta0  = mod(atan2(pp0(:,2), pp0(:,1)) + 2*pi, 2*pi);
theta   = mod(atan2(pp(:,2), pp(:,1)) + 2*pi - theta0, 2*pi);

% convert to degrees
theta = theta * 180 / pi;


function [varargout] = dcblock(varargin);
%
% DCBLOCK   determines the filter coefficient a for the dc blocking/high-
%           pass filter y(t) = x(t) - x(t-1) + a*y(t-1).
%
%           [a] = dcblock(Fc) returns the filter coefficient a for
%           normalized cut-on frequency Fc.
%
%           [a] = dcblock(fc,fs)returns the filter coefficient a for cut-on
%           frequency fc, and sampling frequency fs, when both are given in
%           Hz.
%
%           [aQ] = dcblock(fc,fs,B) returns the quantized filter
%           coefficient aQ when the quantization level B is given.
%
%           dcblock(...) plots the frequency and phase response of the
%           filter.
%
%           Note that if two parameters are passed to the function without
%           an output argument, the plot routine always interprets the
%           first as fc and the second as fs.
%
% SYNTAX    [a] = dcblock(Fc);
%           [a] = dcblock(fc,fs);
%           [aQ] = dcblock(fc,fs,B);
%           [Fc,fc] = dcblock(a,fs);
%           dcblock(Fc)
%           dcblock(fc,fs)
%           dcblock(fc,fs,B)
%
% INPUT     Fc      Normalized frequency cut-on of the filter.
%                   Fc = fc[Hz]/(fs[Hz]/2); fs/2 is the Nyquist frequency.
%                   Note that 0 < Fc < 1/3;
%
%           fc      filter cut-on frequency in Hz (or other reciprocal
%                   units of frequency e.g. 1/metre).
%
%           fs      sampling frequency in Hz (or other reciprocal
%                   units of frequency e.g. 1/metre).
%
%           B       quantization level for fixed point processing e.g. B is
%                   16 for 16-bit operation.
%
% OUTPUT    a       filter coefficient; 0 < a < 1 for floating point
%                   operation.
%
%           aQ      quantized filter coefficient a; 0 < aQ < 2^(B-1) - 1.
%                   The quantization level is set by B which is assumed to
%                   be signed integer.
%
% EXAMPLES  (1) Calculate the filter coefficient a when the normalized
%               frequency is 1E-5:
%
%           >> format long                      % set display format
%           >> a = dcblock(1E-5)                % Fc = 0.00001
%           >> a = 0.99994558749947             % filter coefficient
%
%           (2) Calculate the normalized and true cut-on frequency when the
%               coefficient a is 0.9987 and the sampling frequency is 1MHz:
%
%           >> [Fc,fc] = dcblock(0.9987,1E6)    % a = 0.9987; fs = 1MHz
%           >> Fc = 2.3906E-004                 % this output is normalized
%           >> fc = 119.5323                    % this output is in Hz
%
%           Note the difference in inputs between [Fc,fc] = dcblock(a,fs),
%           the function call [a] = dcblock(fc,fs), and its plot facility
%           dcblock(fc,fs). You can plot the filter response using
%
%           >> dcblock(119.5323,1E6)            % fc = 119.5323; fs = 1E6
%
%           (3) Carry out filtering operation using the dc blocking filter
%               with cut-on frequency of 70Hz and sampling rate of 10kHz.
%
%           t = -0.1:1/10000:0.1-1/10000;       % set up time base
%           s = sin(2*pi*2.5*t+pi/6)+0.01*randn(size(t))+1.25;
%           x = gauspuls(t,250,0.6) + s;        % noisy Gaussian test pulse
%           p = dcblock(70,10000);              % get filter coefficient
%           b = [1 -1];                         % set up differentiator
%           a = [1 -p];                         % set up integrator
%           y = filter(b,a,x);                  % filter test data
%           plot(t,x,t,y)                       % plot results to screen
%           legend('Original','DC Filtered','location','northwest');
%
%           The forward pass of the filter creates a distortion in the
%           Gaussian pulse. This distortion can be removed by using the
%           filtfilt form of the filter. If you now try
%
%           y2 = filtfilt(b,a,x);               % filter test data
%           plot(t,x,t,y,t,y2)                  % plot results to screen
%           legend('Original','DC Filtered','Undistorted','location','northwest');
%
%           you will notice that the symmetry of the Gaussian pulse has
%           been restored.
%
% See also FILTER, FILTFILT, SPTOOL, FDATOOL.

% Author:   J M De Freitas
%           QinetiQ Ltd, Winfrith Technology Centre
%           Winfrith, Dorchester DT2 8XJ, UK.
%
% Email     jdefreitas@qinetiq.com
%
% Revision  1.0
%
% Date      11 October 2006.
%

% Perform checks on input and output
Lin = nargin;
Lout = nargout;
if Lin > 3
    error('dcblock error: Too many input arguments.');
end
if Lin < 1
    errror('dcblock error: No input argument used.');
end
if Lout > 2
    error('dcblock error: Too many output arguments.');
end

% Calculate a when Fc is given and plot response if required
if Lin == 1
    Fc = varargin{1};
    if (Fc < 0)|(Fc > 1/3)
        error('dcblock error: Normalized cut-on frequency must be between 0 and 1/3.');
    end
    a = coef(Fc);
    if Lout == 1
        varargout{1} = a;
    elseif Lout == 0
        PlotResp(1,a);
    end
end

% Calculate a when fc and fs are given and plot response if required
% or Fc and fc when a and fs are given
if Lin == 2
    if (Lout == 1)|(Lout == 0)
        fc = varargin{1};
        fs = varargin{2};
        Fc = 2*fc/fs;
        if (Fc < 0)|(Fc > 1/3)
            error('dcblock error: Cut-on frequency must be between 0 and fs/6.');
        end
        a = coef(Fc);
        if Lout == 1
            varargout{1} = a;
        else
            PlotResp(fs/2,a);
        end
    elseif Lout == 2
        a = varargin{1};
        fs = varargin{2};
        if (a < 0)|(a > 1)
            error('dcblock error: Filter coefficient a must be between 0 and 1.');
        end
        Fc = atan(sqrt(3)*(1-a^2)/(1+4*a+a^2))/pi;
        varargout{1} = Fc;
        varargout{2} = Fc*fs/2;
    end
end

% Calculate a when B, fc and fs are given and plot if required
if Lin == 3
    fc = varargin{1};
    fs = varargin{2};
    B = floor(varargin{3});
    Fc = 2*fc/fs;
    if (Fc < 0)|(Fc > 1/3)
        error('dcblock error: Cut-on frequency fc must be between 0 and fs/6.');
        return
    end
    if (B <= 2)|(B > 300)
        error('dcblock error: Quantization level B must be between 2 and 300.');
        return
    end
    a = coef(Fc);
    if Lout == 1
        varargout{1} = floor(a*(2^(B-1)-1));
    elseif Lout == 0
        aQ = floor(a*(2^(B-1)-1))/(2^(B-1)-1);
        PlotResp(fs/2,aQ);
    end
end

function a = coef(Fc);
% get up coefficient
    a = (sqrt(3) - 2*sin(pi*Fc))/(sin(pi*Fc) + sqrt(3)*cos(pi*Fc));

function PlotResp(Fn,a);
% Plot frequency and phase response
    f = 0.000001:0.000001:1-0.000001;
    h = (1+a)*sin(pi*f/2)./sqrt(1+a^2-2*a*cos(pi*f));
    phi = atan((1-a)*(sin(pi*f)./(1-cos(pi*f)))/(1+a));
    indigo = [.48 .06 .89];
    subplot(2,1,1);
        loglog(Fn*f,h,'LineWidth',1.5,'Color',indigo);
        title('\bf Frequency Response: IIR DC Blocking Filter','Color','k');
        if Fn == 1
            xlabel('Normalized Frequency');
        else
            xlabel('Frequency (Hz)');
        end
        ylabel('Magnitude ');
        minY = min(get(gca,'YTick'));
        minX = min(get(gca,'XTick'));
        maxX = max(get(gca,'XTick'));
        if minY >= 0.1
            TickY = [0.1 1];
        else
            for j = 1:abs(log10(minY))+1 TickY(j) = 10^(log10(minY)+j-1); end
        end
        for j = 1:(log10(maxX)-log10(minX))+1 TickX(j) = 10^(log10(minX)+j-1); end
        ylim([TickY(1) 1.1]);
        set(gca,'XGrid','on','YGrid','on','XMinorGrid','on',...
        'YMinorGrid','on','GridLineStyle','-',...
        'MinorGridLineStyle','-','Xtick',TickX,'Ytick',TickY,...
        'Color',[0.835 0.918 0.918]);
    subplot(2,1,2);
        loglog(Fn*f,phi*180/pi,'LineWidth',1.5,'Color',indigo);
        title('\bf Phase Response: IIR DC Blocking Filter','Color','k');
        if Fn == 1
            xlabel('Normalized Frequency');
        else
            xlabel('Frequency (Hz)');
        end
        ylabel('Phase (degree) ');
        ylim([0.1 100]);
        minX = min(get(gca,'XTick'));
        maxX = max(get(gca,'XTick'));
        for j = 1:(log10(maxX)-log10(minX))+1
            TickX2(j) = minX*10^(j-1) ;
        end
        set(gca,'XGrid','on','YGrid','on','XMinorGrid','on',...
        'YMinorGrid','on','GridLineStyle','-',...
        'MinorGridLineStyle','-','Xtick',TickX2,'YTick',[0.1 1 10 100],...
        'Color',[0.835 0.918 0.918]);


%  Script file: calc_roots2.m
%
%  Purpose:
%    This program solves for the roots of a quadratic equation
%    of the form a*x**2 + b*x + c = 0.  It calculates the answers
%    regardless of the type of roots that the equation possesses.
%
%  Record of revisions:
%      Date       Programmer          Description of change
%      ====       ==========          =====================
%    02/24/07    S. J. Chapman        Original code
%
% Define variables:
%   a            -- Coefficient of x^2 term of equation
%   b            -- Coefficient of x term of equation
%   c            -- Constant term of equation
%   discriminant -- Discriminant of the equation
%   x1           -- First solution of equation
%   x2           -- Second solution of equation

% Prompt the user for the coefficients of the equation
disp ('This program solves for the roots of a quadratic ');
disp ('equation of the form A*X^2 + B*X + C = 0. ');
a = input ('Enter the coefficient A: ');
b = input ('Enter the coefficient B: ');
c = input ('Enter the coefficient C: ');

% Calculate discriminant
discriminant = b^2 - 4 * a * c;

% Solve for the roots
x1 = ( -b + sqrt(discriminant) ) / ( 2 * a );
x2 = ( -b - sqrt(discriminant) ) / ( 2 * a );

% Display results
disp ('The roots of this equation are:');
fprintf ('x1 = (%f) +i (%f)\n', real(x1), imag(x1));
fprintf ('x2 = (%f) +i (%f)\n', real(x2), imag(x2));


function f=dctresample(f,L,dim)
%DCTRESAMPLE   Resample signal using Fourier interpolation
%   Usage:  h=dctresample(f,L);
%           h=dctresample(f,L,dim);
%
%   `dctresample(f,L)` returns a discrete cosine interpolation of the signal *f*
%   to length *L*. If the function is applied to a matrix, it will apply
%   to each column.
%
%   `dctresample(f,L,dim)` does the same along dimension *dim*.
%
%   If the input signal is not a periodic signal (or close to), this method
%   will give much better results than |fftresample| at the endpoints, as
%   this method assumes than the signal is even a the endpoints.
%
%   The algorithm uses a DCT type iii.
%
%   See also:  fftresample, middlepad, dctiii

%   AUTHOR: Peter L. Søndergaard

% ------- Checking of input --------------------
complainif_argnonotinrange(nargin,2,3,mfilename);

if nargin<3
  dim=[];
end;

[f,L,Ls,W,dim,permutedsize,order]=assert_sigreshape_pre(f,L,dim,'DCTRESAMPLE');

wasreal=isreal(f);

% The 'dim=1' below have been added to avoid dct and middlepad being
% smart about choosing the dimension.
f=dctiii(postpad(dctii(f,[],1),L))*sqrt(L/Ls);

f=assert_sigreshape_post(f,dim,permutedsize,order);

if wasreal
  f=real(f);
end;


function f1 = p07_f1 ( x )

%*****************************************************************************80
%
%% P07_F1 evaluates the first derivative for problem 7.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the value of the variable.
%
%    Output, real F1, the first derivative of the
%    objective function.
%
  f1 = ( 1.0 - 2.0 * x * x + cos ( x ) ...
         - 2.0 * x * sin ( x ) ) * exp ( - x * x );

  return
end


function inside = sphere_imp_contains_point_3d ( r, center, p )

%*****************************************************************************80
%
%% SPHERE_IMP_CONTAINS_POINT_3D: point in implicit sphere in 3D?
%
%  Discussion:
%
%    An implicit sphere in 3D satisfies the equation:
%
%      sum ( ( P(1:DIM_NUM) - CENTER(1:DIM_NUM) )**2 ) = R**2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the sphere.
%
%    Input, real CENTER(3), the center of the sphere.
%
%    Input, real P(3), the point to be checked.
%
%    Output, logical INSIDE, is TRUE if the point is inside the sphere.
%
  dim_num = 3;

  if ( sum ( ( p(1:dim_num) - center(1:dim_num) ).^2 ) <= r * r )
    inside = 1;
  else
    inside = 0;
  end

  return
end


function er4OC
%EG4OC    Example 4 of optimal control tutorial.
%    This example is from the following reference:
%    S.N.Avvakumov and Yu.N.Kiselev, "Boundary value problem for ordinary
%    differential equations with applications to optimal control".
%    Example 5 on page 8

global mu;
mu = 0.5;
solinit = bvpinit(linspace(0,1,10),[2;2;0;-1],4);

sol = bvp4c(@ode,@bc,solinit);

figure(1)
lines = {'k-.','r--','b-'};
ut = sol.y(4,:)./sqrt(mu*sol.y(3,:).^2+sol.y(4,:).^2);
t = sol.parameters*sol.x;
plot(t,ut,lines{1});
% axis([-0.1 1.1 0 1.7]);
title('Smoothed control');
xlabel('t');
ylabel('u');
drawnow; hold on;

figure(2)
plot(sol.y(1,:),sol.y(2,:),lines{1});
% axis([-0.1 1.1 0 1.7]);
title('Admissible trajectories');
xlabel('x_1');
ylabel('x_2');
drawnow; hold on;

% the solution for one value of mu is used as guess for the next.
for i=2:3
  if i==2
      mu = 0.3;
  else
      mu = .1;
  end;
  % After creating function handles, the new value of mu
  % will be used in nested functions.
  sol = bvp4c(@ode,@bc,sol);
  figure(2)
  plot(sol.y(1,:),sol.y(2,:),lines{i});
  drawnow;

  figure(1)
  ut = sol.y(4,:)./sqrt(mu*sol.y(3,:).^2+sol.y(4,:).^2);
  t = sol.parameters*sol.x;
  plot(t,ut,lines{i});
  drawnow;

end
figure(1); legend('mu = 0.5', 'mu = 0.3', 'mu = 0.1', ...
                  'Location', 'NorthWest'); hold off;
figure(2); legend('mu = 0.5', 'mu = 0.3', 'mu = 0.1', ...
                  'Location', 'NorthWest'); hold off;

% print -djpeg90 -f1 -r300 eg4_trajectory.jpg;
% print -djpeg90 -f2 -r300 eg4_control.jpg

% --------------------------------------------------------------------------
function dydt = ode(t,y,T)
global mu;
term = sqrt(mu*y(3)^2+y(4)^2);
dydt = T*[ y(2) + mu*y(3)/term
           y(4)/term
           0
           -y(3)];

% --------------------------------------------------------------------------
% boundary conditions, with 4 states and 1 parameters, 5 conditions are
% needed: x1(0) =2, x2(0) = 2; x1(1) = 0; x2(1) = 0; x3(1)^2+x4(1)^2 = 1;
function res = bc(ya,yb,T)
res = [ya(1)-2
       ya(2)-2
       yb(1)
       yb(2)
       yb(3)^2+yb(4)^2-1];


function [v,unv] = statmoments(p,n)
%STATMOMENTS Computes statistical central moments of image histogram.
%   [W,UNV] = STATMOMENTS(P,N) computes up to the Nth statistical
%   central moment of a histogram whose components are in vector
%   P. The length of P must equal 256 or 65536.
%
%   The program outputs a vector V with V(1) = mean, V(2) = variance,
%   V(3) = 3rd moment, . . . V(N) = Nth central moment. The random
%   variable values are normalized to the range [0,1], so all
%   moments also are in this range.
%
%   The program also outputs a vector UNV containing the same moments
%   as V, but using un-normalized random variable values (e.g., 0 to
%   255 if length(P) = 2^8). For example, if length(P) = 256 and V(1)
%   = 0.5, then UNV(1) would have the value UNV(1) = 127.5 (half of
%   the [0,255] range).
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

Lp = length(p);
if (Lp ~= 256) && (Lp ~= 65536)
   error('P must be a 256- or 65536-element vector.');
end
G = Lp - 1;

% Make sure the histogram has unit area, and convert it to a
% column vector.
p = p/sum(p); p = p(:);

% Form a vector of all the possible values of the
% random variable.
z = 0:G;

% Now normalize the z's to the range [0,1].
z = z./G;

% The mean.
m = z*p;

% Center random variables about the mean.
z = z - m;

% Compute the central moments.
v = zeros(1,n);
v(1) = m;
for j = 2:n
   v(j) = (z.^j)*p;
end

if nargout > 1
   % Compute the uncentralized moments.
   unv = zeros(1,n);
   unv(1)=m.*G;
   for j = 2:n
      unv(j) = ((z*G).^j)*p;
   end
end


function s = legendre_integral ( p )

%*****************************************************************************80
%
%% LEGENDRE_INTEGRAL evaluates a monomial Legendre integral.
%
%  Discussion:
%
%    To test a Legendre quadrature rule, we use it to approximate the
%    integral of a monomial:
%
%      integral ( -1 <= x <= +1 ) x^p dx
%
%    This routine is given the value of the exponent, and returns the
%    exact value of the integral.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer P, the power.
%
%    Output, real S, the value of the exact integral.
%

%
%  Get the exact value of the integral.
%
  if ( mod ( p, 2 ) == 0 )

    s = 2.0 / ( p + 1 );

  else

    s = 0.0;

  end

  return
end


function value = torus_square_area_3d ( r1, r2 )

%*****************************************************************************80
%
%% TORUS_SQUARE_AREA_3D returns the area of a square torus in 3D.
%
%  Integration region:
%
%    Points (X,Y,Z) such that:
%
%      R1 - R2 <= SQRT ( X**2 + Y**2 ) <= R1 + R2,
%      -R2 <= Z <= R2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R1, R2, the two radii that define the torus.
%
%    Output, real TORUS_SQUARE_AREA_3D, the area of the torus.
%
  value = 16.0E+00 * pi * r1 * r2;

  return
end


function mean = genlogistic_mean ( a, b, c )

%*****************************************************************************80
%
%% GENLOGISTIC_MEAN returns the mean of the Generalized Logistic PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < B,
%    0.0 < C.
%
%    Output, real MEAN, the mean of the PDF.
%
  mean = a + b * ( euler_constant ( ) + digamma ( c ) );

  return
end


function [R,subgrad,data] = risk_svm(data,W)
% RISK_SVM evaluates SVM risk term given by L1-hinge loss.
%
% Synopsis:
%  [R,subgrad,data] = risk_svm(data)
%  [R,subgrad,data] = risk_svm(data,W)
%
% Description:
%  Let the risk term be defined as
%
%  R(W) = 1/m sum_{i=1}^m max(0, 1-data.y(i)*(W'*[data.X(:,i);data.X0]))
%
%  This function returns value R and subgradient SUBGRAD of the
%  risk R(W) at W.
%

if nargin < 2
    % evaluates the risk and the subgradient of at W = 0

    nExamples = size(data.X,2);
    R = 1;
    if data.X0 == 0
        subgrad = -data.X*data.y(:)/nExamples;
    else
        subgrad = [-data.X*data.y(:)/nExamples; ...
                   -data.X0*sum(data.y)/nExamples];
    end

else

    nExamples = size(data.X,2);

    if data.X0 == 0
        dfce = 1 - (W'*data.X).*reshape(data.y,1,nExamples);

        idx = find(dfce>0);
        R = sum(dfce(idx))/nExamples;

        subgrad = -data.X(:,idx)*reshape(data.y(idx),length(idx),1)/nExamples;
    else

        dfce = 1 - (W(1:end-1)'*data.X + W(end)*data.X0).*(data.y(:)');
        idx = find(dfce>0);

        R = sum(dfce(idx))/nExamples;
        subgrad = [-data.X(:,idx)*reshape(data.y(idx),length(idx),1)/nExamples; ...
                   -data.X0*sum(data.y(idx))/nExamples];
    end

end

return;
% EOF


% compute abscissas and weight factors for Gaussian-Hermite quadrature
%
% CALL:  [x,w] = gauher(N)
%
%  x = base points (abscissas)
%  w = weight factors
%  N = number of base points (abscissas) (integrates an up to (2N-1)th order
%      polynomial exactly)
%
%  p(x)=exp(-x^2/2)/sqrt(2*pi), a =-Inf, b = Inf
%
%  The Gaussian Quadrature integrates a (2n-1)th order
%  polynomial exactly and the integral is of the form
%           b                         N
%          Int ( p(x)* F(x) ) dx  =  Sum ( w_j* F( x_j ) )
%           a                        j=1
%
%      this procedure uses the coefficients a(j), b(j) of the
%      recurrence relation
%
%           b p (x) = (x - a ) p   (x) - b   p   (x)
%            j j            j   j-1       j-1 j-2
%
%      for the various classical (normalized) orthogonal polynomials,
%      and the zero-th moment
%
%           1 = integral w(x) dx
%
%      of the given polynomial's weight function w(x).  Since the
%      polynomials are orthonormalized, the tridiagonal matrix is
%      guaranteed to be symmetric.

function [x,w]=gauher(N)
    if N==20 % return precalculated values
        x=[ -7.619048541679757;-6.510590157013656;-5.578738805893203;
            -4.734581334046057;-3.943967350657318;-3.18901481655339 ;
            -2.458663611172367;-1.745247320814127;-1.042945348802751;
            -0.346964157081356; 0.346964157081356; 1.042945348802751;
             1.745247320814127; 2.458663611172367; 3.18901481655339 ;
             3.943967350657316; 4.734581334046057; 5.578738805893202;
             6.510590157013653; 7.619048541679757];
        w=[  0.000000000000126; 0.000000000248206; 0.000000061274903;
             0.00000440212109 ; 0.000128826279962; 0.00183010313108 ;
             0.013997837447101; 0.061506372063977; 0.161739333984   ;
             0.260793063449555; 0.260793063449555; 0.161739333984   ;
             0.061506372063977; 0.013997837447101; 0.00183010313108 ;
             0.000128826279962; 0.00000440212109 ; 0.000000061274903;
             0.000000000248206; 0.000000000000126 ];
    else
        b = sqrt( (1:N-1)/2 )';
        [V,D] = eig( diag(b,1) + diag(b,-1) );
        w = V(1,:)'.^2;
        x = sqrt(2)*diag(D);
    end


function cPoly=composePolys(a,b)
%%COMPOSEPOLYS Compose two polynomials given as power series. This means,
%              for polynomials A(x) and B(x) in a scalar x, evaluate the
%              composition C(x)=A(B(x)). C(x) itself is a polynomial. Thus,
%              the output of the function is the set of coefficients of C
%              (subject to finite precision limitations in the recursion).
%
%INPUTS: a Coefficients for a power series of the form
%          y(z)=a(end)+a(end-1)*z+a(end-2)*z^2+...
%          into which the power series in b is to be substituted. The
%          ordering of the coefficients is the same as in Matlab's polyval
%          function.
%        b Coefficients for the power series that is substitued into a.
%
%OUTPUTS: c Coefficients for the power series that represents the
%           substitution of the polynomial b into a.
%
%The substitution is performed by explicitly evaluating power of b (via
%repeated convolutions), multiplying the powers by the appropriate value in
%a and summing the results.
%
%February 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

orderA=length(a)-1;
orderB=length(b)-1;

%The order of the output.
prodOrder=orderA*orderB;

%Zero-pad b so that it is the same length as the length of the output. This
%simplifies assignments in the loop.
b=[zeros(prodOrder-orderB,1);b(:)];

%bProd will hold powers of b. Right now, it is just initialized to 1 (b^0).
bProd=[zeros(prodOrder,1);1];

%Initialize the output with the zeroth-order term.
cPoly=zeros(prodOrder+1,1);
cPoly=cPoly+a(end)*bProd;
for curPow=1:orderA
    %Get the polynomial for the next power of b. The convolution operation
    %is equivalent to polynomial multiplication. The extra leading terms
    %(which are all zeros) are just thrown out.
    bProd=conv(b,bProd);
    bProd=bProd((prodOrder+1):end);

    %Add in the next power term.
    cPoly=cPoly+a(end-curPow)*bProd;
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 result = ball_unit_f1_nd ( func, n )

%*****************************************************************************80
%
%% BALL_UNIT_F1_ND approximates an integral inside a unit ball in ND.
%
%  Integration region:
%
%    Points X(1:N) such that:
%
%      Sum ( X(1:N)**2 ) <= 1.
%
%  Discussion:
%
%    An (N+1)*2**N point 5-th degree formula is used, Stroud number SN:5-6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 November 2004
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F at the N-vector X, of the form
%      function value = func ( n, x )
%
%    Input, integer N, the dimension of the space.
%
%    Output, real RESULT, the approximate integral of the function.
%
  u2 = ( 1.0E+00 - 2.0E+00 * sqrt ( 1.0E+00 / ( n + 4 ) ) ) / ( n + 2 );
  u = sqrt ( u2 );
  x(1:n) = - u;

  w = 1.0E+00 / ( ( n + 1 ) * 2^n );

  quad = 0.0E+00;
  ihi = 2^n;

  for i = 1 : ihi

    itemp = i - 1;

    for j = 1 : n

      if ( mod ( itemp, 2 ) == 1 )
        x(j) = - abs ( x(j) );
      else
        x(j) = abs ( x(j) );
      end

      itemp = floor ( itemp / 2 );

    end

    quad = quad + w * feval ( func, n, x );

  end

  temp = sqrt ( n + 4 );

  t = sqrt ( 2.0E+00 * ( n + 1 ) / ( n + 2 ) ) / ( n * temp );

  y = ( 1.0E+00 + 2.0E+00 / ( n * temp ) ) / ( n + 2 );
  v = sqrt ( y - t );
  u = sqrt ( y + ( n - 1 ) * t );

  khi = 2^n;

  for i = 1 : n

    x(1:n) = - v;

    x(i) = - u;

    for k = 1 : khi

      ktemp = k - 1;

      for j = 1 : n

        if ( mod ( ktemp, 2 ) == 1 )
          x(j) = - abs ( x(j) );
        else
          x(j) = abs ( x(j) );
        end

        ktemp = floor ( ktemp / 2 );

      end

      quad = quad + w * feval ( func, n, x );

    end

    x(i) = - v;

  end

  volume = ball_unit_volume_nd ( n );
  result = quad * volume;

  return
end


%% Machine Learning Online Class
%  Exercise 1: Linear regression with multiple variables
%
%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  linear regression exercise.
%
%  You will need to complete the following functions in this
%  exericse:
%
%     warmUpExercise.m
%     plotData.m
%     gradientDescent.m
%     computeCost.m
%     gradientDescentMulti.m
%     computeCostMulti.m
%     featureNormalize.m
%     normalEqn.m
%
%  For this part of the exercise, you will need to change some
%  parts of the code below for various experiments (e.g., changing
%  learning rates).
%

%% Initialization

%% ================ Part 1: Feature Normalization ================

%% Clear and Close Figures
clear ; close all; clc

fprintf('Loading data ...\n');

%% Load Data
data = load('ex1data2.txt');
X = data(:, 1:2);
y = data(:, 3);
m = length(y);

% Print out some data points
fprintf('First 10 examples from the dataset: \n');
fprintf(' x = [%.0f %.0f], y = %.0f \n', [X(1:10,:) y(1:10,:)]');

fprintf('Program paused. Press enter to continue.\n');
pause;

% Scale features and set them to zero mean
fprintf('Normalizing Features ...\n');

[X mu sigma] = featureNormalize(X);

% Add intercept term to X
X = [ones(m, 1) X];

%% ================ Part 2: Gradient Descent ================

% ====================== YOUR CODE HERE ======================
% Instructions: We have provided you with the following starter
%               code that runs gradient descent with a particular
%               learning rate (alpha).
%
%               Your task is to first make sure that your functions -
%               computeCost and gradientDescent already work with
%               this starter code and support multiple variables.
%
%               After that, try running gradient descent with
%               different values of alpha and see which one gives
%               you the best result.
%
%               Finally, you should complete the code at the end
%               to predict the price of a 1650 sq-ft, 3 br house.
%
% Hint: By using the 'hold on' command, you can plot multiple
%       graphs on the same figure.
%
% Hint: At prediction, make sure you do the same feature normalization.
%

fprintf('Running gradient descent ...\n');

% Choose some alpha value
alpha = 0.01;
num_iters = 400;

% Init Theta and Run Gradient Descent
theta = zeros(3, 1);
[theta, J_history] = gradientDescentMulti(X, y, theta, alpha, num_iters);

% Plot the convergence graph
figure;
plot(1:numel(J_history), J_history, '-b', 'LineWidth', 2);
xlabel('Number of iterations');
ylabel('Cost J');

% Display gradient descent's result
fprintf('Theta computed from gradient descent: \n');
fprintf(' %f \n', theta);
fprintf('\n');

% Estimate the price of a 1650 sq-ft, 3 br house
% ====================== YOUR CODE HERE ======================
% Recall that the first column of X is all-ones. Thus, it does
% not need to be normalized.
price = 0; % You should change this

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

fprintf(['Predicted price of a 1650 sq-ft, 3 br house ' ...
         '(using gradient descent):\n $%f\n'], price);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% ================ Part 3: Normal Equations ================

fprintf('Solving with normal equations...\n');

% ====================== YOUR CODE HERE ======================
% Instructions: The following code computes the closed form
%               solution for linear regression using the normal
%               equations. You should complete the code in
%               normalEqn.m
%
%               After doing so, you should complete this code
%               to predict the price of a 1650 sq-ft, 3 br house.
%

%% Load Data
data = csvread('ex1data2.txt');
X = data(:, 1:2);
y = data(:, 3);
m = length(y);

% Add intercept term to X
X = [ones(m, 1) X];

% Calculate the parameters from the normal equation
theta = normalEqn(X, y);

% Display normal equation's result
fprintf('Theta computed from the normal equations: \n');
fprintf(' %f \n', theta);
fprintf('\n');

% Estimate the price of a 1650 sq-ft, 3 br house
% ====================== YOUR CODE HERE ======================
price = 0; % You should change this

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

fprintf(['Predicted price of a 1650 sq-ft, 3 br house ' ...
         '(using normal equations):\n $%f\n'], price);