function basis = basis_col(A)
%
% Bases
%
% basis_col(A) produces a basis for the subspace of Eucldiean n-space
% spanned by the vectors {u1,u2,...}, where the matrix A is formed from
% these vectors as its columns. That is, the subspace is the column space
% of A. The columns of the matrix that is returned are the basis vectors
% for the subspace. These basis vectors will be a subset of the original
% vectors. An error is returned if a basis for the zero vector space is
% attempted to be produced.
%
% For example, if the vector space V = span{u1,u2,...}, where u1,u2,... are
% row vectors, then set A to be [u1' u2' ...].
%
% For example, if the vector space V = Col(B), where B is an m x n matrix,
% then set A to be equal to B.

matrix_size = size(A);

m = matrix_size(1,1);
n = matrix_size(1,2);

if A == zeros(m,n)
    error('There does not exist a basis for the zero vector space.');
elseif n == 1
    basis = A;
else
    flag = 0;

    if n == 2
        multiple = A(1,2)/A(1,1);
        count = 0;

        for i = 1:m
            if A(i,2)/A(i,1) == multiple
                count = count + 1;
            end
        end

        if count == m
            basis = A(1:m,1);
            flag = 1;
        end
    end

    if flag == 0
        [ref_A pivot_columns] = ref(A);

        for i = 1:size(pivot_columns,2)
            B(1:m,i) = A(1:m,pivot_columns(1,i));
        end

        basis = B;
    end
end


function [ vX, mP ] = ApplyKalmanFilterIteration( vX, mP, vZ, hF, hH, hMf, hMh, mQ, mR )
% ----------------------------------------------------------------------------------------------- %
% [ vX, mP ] = ApplyKalmanFilterIteration( vX, mP, vZ, hF, hH, mQ, mR, mF, mH )
%   Applies iteration of the Kalman Filter (Prediction + Update). Supports
%   both Linear and Non Linear Mode (Extended Kalman Filter).
% Input:
%   - vX            -   Input State Vector.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - mP            -   Error Covariance.
%                       Structure: Matrix.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - vZ            -   Measurement Vector.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - hF            -   Model Function.
%                       Structure: Function Handler.
%                       Type: Function Handler.
%                       Range: NA.
%   - hH            -   Measurement Function.
%                       Structure: Function Handler.
%                       Type: Function Handler.
%                       Range: NA.
%   - mQ            -   Process Noise Covariance.
%                       Structure: Matrix.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - mR            -   Measurement Noise Covariance.
%                       Structure: Matrix.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - hMf           -   Model Matrix Function.
%                       Generates the matrix `mF` from the state vector vX.
%                       Structure: Function Handler.
%                       Type: Function Handler.
%                       Range: NA.
%   - hMh           -   Measurement Matrix Function.
%                       Generates the matrix `mH` from the state vector vX.
%                       The state vector is hF(vX).
%                       Structure: Function Handler.
%                       Type: Function Handler.
%                       Range: NA.
% Output:
%   - vX            -   Updated State Vector.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - mP            -   Error Covariance.
%                       Structure: Matrix.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  Extended Kalman Filter (Wikipedia) - https://en.wikipedia.org/wiki/Extended_Kalman_filter.
%   2.  Kalman Filter (Wikipedia) - https://en.wikipedia.org/wiki/Kalman_filter.
% Remarks:
%   1.  If the Complex Mode is selected the function must return complex
%       values in order to work. For instance, if the input function is
%       'norm(vX)' use 'sum(vX .^ 2)' and if the input function is
%       'sum(abs(vX))' use 'sum(sqrt(vX .^ 2))'.
%   2.  This implementation use the Joseph Form of the Covariance Update.
%       While it requires more computational effort it is more stable and
%       guaranteed to generate PSD matrix.
% TODO:
%   1.  U.
% Release Notes:
%   -   1.1.000     25/07/2021  Royi Avital
%       *   Added support for a fnction handle to calculate the matrices mF
%           and mH.
%       *   Add `arguments` block.
%       *   Added a step to ensure symmetry of mP.
%   -   1.0.000     22/08/2018  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

arguments
    vX (:, 1) {mustBeNumeric, mustBeReal}
    mP (:, :) {mustBeNumeric, mustBeReal, mustBePdMatrix}
    vZ (:, 1) {mustBeNumeric, mustBeReal}
    hF (1, 1) {mustBeFunctionHandler}
    hH (1, 1) {mustBeFunctionHandler}
    hMf (1, 1) {mustBeFunctionHandler}
    hMh (1, 1) {mustBeFunctionHandler}
    mQ (:, :) {mustBeNumeric, mustBeReal, mustBePdMatrix}
    mR (:, :) {mustBeNumeric, mustBeReal, mustBePdMatrix}
end

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

mI = eye(size(vX, 1));

% Prediction Step
vX = hF(vX);
mF = hMf(vX);
mP = (mF * mP * mF.') + mQ;

mH = hMh(vX);

% Update Step
vY = vZ - hH(vX);
mS = (mH * mP * mH.') + mR;
mK = (mP * mH.') / mS;
vX = vX + (mK * vY);
% Joseph Form of the Covariance Update (Numerically more stable)
mT = mI - (mK * mH);
mP = (mT * mP * mT.') + (mK * mR * mK.'); %<! Joseph Form
mP = (mP.' + mP) / 2; %<! Ensure symmetry

end

function [ ] = mustBePdMatrix( mX )
% https://www.mathworks.com/matlabcentral/answers/107552
if(issymmetric(mX) && (all(eig(mX) > 0)))

else
    errorId     = 'mustBePdMatrix:notPositiveDefiniteMatrix';
    errorMsg    = 'The input must be a Positive Definite Matrix';
    throwAsCaller(MException(errorId, errorMsg));
end

end

function [ ] = mustBeFunctionHandler( hF )
% https://www.mathworks.com/matlabcentral/answers/107552
if(~isa(hF, 'function_handle'))
    errorId     = 'mustBeFunctionHandler:notFunctionHandler';
    errorMsg    = 'The input must be a Function Handler';
    throwAsCaller(MException(errorId, errorMsg));
end

end


function row_new = catalan_row_next ( ido, n, row )

%*****************************************************************************80
%
%% CATALAN_ROW computes row N of Catalan's triangle.
%
%  Example:
%
%    I\J 0   1   2   3   4   5   6
%
%    0   1
%    1   1   1
%    2   1   2   2
%    3   1   3   5   5
%    4   1   4   9  14  14
%    5   1   5  14  28  42  42
%    6   1   6  20  48  90 132 132
%
%  Recursion:
%
%    C(0,0) = 1
%    C(I,0) = 1
%    C(I,J) = 0 for I < J
%    C(I,J) = C(I,J-1) + C(I-1,J)
%    C(I,I) is the I-th Catalan number.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer IDO, indicates whether this is a call for
%    the 'next' row of the triangle.
%    IDO = 0, this is a startup call.  Row N is desired, but
%    presumably this is a first call, or row N-1 was not computed
%    on the previous call.
%    IDO = 1, this is not the first call, and row N-1 was computed
%    on the previous call.  In this case, much work can be saved
%    by using the information from the previous values of IROW
%    to build the next values.
%
%    Input, integer N, the index of the row of the triangle desired.
%
%    Input, integer ROW(1:N), the previous row of coefficients, if
%    IDO = 1.
%
%    Output, integer ROW_NEW(1:N+1), the next row of coefficients.
%
  if ( n < 0 )
    row_new = [];
    return
  end

  if ( ido == 0 )

    row_new(1) = 1;
    row_new(2:n+1) = 0;

    for i = 1 : n

      row_new(1) = 1;

      for j = 1 : i-1
        row_new(j+1) = row_new(j+1) + row_new(j);
      end

      row_new(i+1) = row_new(i);

    end

  else

    row_new(1:n) = row(1:n);
    row_new(n+1) = 0;

    row_new(1) = 1;

    for j = 1 : n-1
      row_new(j+1) = row_new(j+1) + row_new(j);
    end

    if ( 1 <= n )
      row_new(n+1) = row_new(n);
    end

  end

  return
end


function r8but_print ( n, mu, a, title )

%*****************************************************************************80
%
%% R8BUT_PRINT prints a R8BUT matrix.
%
%  Discussion:
%
%    The R8BUT storage format is used for a banded upper triangular matrix.
%    The matrix is assumed to be zero above the MU-th superdiagonal.
%    The matrix is stored in an MU+1 by N array.
%    Columns are preserved.
%
%    The diagonal is stored in row MU+1 of the array.
%    The first superdiagonal in row MU, columns 2 through N.
%    The second superdiagonal in row MU-1, columns 3 through N.
%    The MU-th superdiagonal in row 1, columns MU+1 through N.
%
%  Example:
%
%    N = 5, MU = 2
%
%    A11 A12 A13   0   0
%      0 A22 A23 A24   0
%      0   0 A33 A34 A35
%      0   0   0 A44 A45
%      0   0   0   0 A55
%                --- ---
%                    ---
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer MU, the upper bandwidth.
%
%    Input, real A(MU+1,N), the R8BUT matrix.
%
%    Input, string TITLE, a title to be printed.
%
  r8but_print_some ( n, mu, a, 1, 1, n, n, title );

  return
end


%% Floyd algorithm
function [S, P]=FloydSPR(AdjMax)
% *INPUT:*
% AdjMax: Adjacent matrix that represents a weighted, directed graph
%
% *OUTPUT:*
% S: distance to destination node
% P: next hop node
%
% *DESCRIPTION*
% Given a input adjacent matrix (AdjMax) that represents a weighted, directed graph.
% The function finds the shorest path from one vertex 'i' to another 'j'.
% The return values includes a matrix (S) that denotes the shortest distance
% between vertices 'i' and 'j', and a matrix (P) that denotes the next vertex 'k'
% on the path from vertex 'i' to vertex 'j'

N=min(length(AdjMax(:,1)),length(AdjMax(1,:)));
P=-1*ones(N,N);
S=AdjMax;

for k=1:N
    for i=1:N
        for j=1:N
            if S(i,k)==inf
                continue;
            end
            if S(k,j)==inf
                continue;
            end
            if S(i,j)>S(i,k)+S(k,j)
                if P(i,k)==-1
                    P(i,j)=k;
                else
                    P(i,j)=P(i,k);
                end
                S(i,j)=S(i,k)+S(k,j);
            end
        end
    end
end

%% END


function zp=nearestPointOnPlane(z,n,p0)
%%NEARESTPOINTONPLANE Given a point z, determine the nearest point on the
%           plane given by the equation n'*(x-p0)=0, where n is a normal to
%           the plane, p0 is a point and x is the free variable. All
%           quantities are real.
%
%INPUTS: z A numDimXnumPoints set of points to project.
%        n A numDimX1 normal to the plane.
%       p0 A numDimX1 point on the plane.
%
%OUTPUTS: zp The numDimXnumPoints set of projections of the points in z
%            onto the plane.
%
%The expression for the nearest point to a plane is given in Chapter 12.1
%of [1].
%
%EXAMPLE:
% n=[1;2;3];
% p0=[0;-1;2];
% z=[4;4;4];
% zp=nearestPointOnPlane(z,n,p0)
% %One will get zp=[2.571428571428572;1.142857142857143;-0.285714285714286].
% %It can be verified that
% n'*(zp-p0)
% %is zero within finite precision limits and
% cross(n,(z-zp))
% %is also zero within finite precision limits.
%
%REFERENCES:
%[1] P. J. Schneider and D. H. Eberly, Geometric Tools for Computer
%    Graphics. Amsterdam: Morgan Kaufmann Publishers, 2003.
%
%April 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

d=-n'*p0;
%The equation for the plane is now:
%x'*n+d=0;

u=n/(n'*n);

%For a single point z, the following line is equivalent to
%zp=z-(z'*n+d)*u;
zp=bsxfun(@minus,z,bsxfun(@times,sum(bsxfun(@times,z,n),1)+d,u));

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.


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% Problem 5
%                                    0.9^n, 0<=n<=3
%  z-Transform computation of f[n]=  2^-n , 4<=n<=6
%                                     1,    7<=n<=10

n1=0:3;
f1=0.9.^n1;
n2=4:6;
f2=2.^(-n2);
n3=7:10;
f3=ones(size(n3));
n=[n1 n2 n3];
f=[f1 f2 f3];
stem(n,f);
axis([-.5 10.5 -.1 1.1]);

figure
syms n z
n1=4;
n2=7;
n3=11;
f1=0.9^n;
f2=2^(-n);
f3=1;
f=f1*(heaviside(n)-heaviside(n-n1)) +f2*(heaviside(n-n1)-heaviside(n-n2)) +f3*(heaviside(n-n2)-heaviside(n-n3));
n_s=0:10;
f_s=subs(f,n_s);
stem(n_s,f_s);
axis([-.5 10.5 -.1 1.1]);

% a)z-Transform of f[n]
F1=ztrans(f,z)

% b)z-Transform of f[n]
n1=0:3;
f1=0.9.^n1;
n2=4:6;
f2=2.^(-n2);
n3=7:10;
f3=ones(size(n3));
n=[n1 n2 n3];
f=[f1 f2 f3];
F2=sum(f.*(z.^-n))

% c) confirmation
figure
syms n
ftest=iztrans(F1,n)
n=0:10;
ftest1=subs(ftest,n);
stem(n,ftest1);
axis([-.5 10.5 -.1 1.1]);


function v3 = r8vec_cross_product_3d ( v1, v2 )

%*****************************************************************************80
%
%% R8VEC_CROSS_PRODUCT_3D computes the cross product of two R8VEC's in 3D.
%
%  Discussion:
%
%    The cross product in 3D can be regarded as the determinant of the
%    symbolic matrix:
%
%          [  i  j  k ]
%      det [ x1 y1 z1 ]
%          [ x2 y2 z2 ]
%
%      = ( y1 * z2 - z1 * y2 ) * i
%      + ( z1 * x2 - x1 * z2 ) * j
%      + ( x1 * y2 - y1 * x2 ) * k
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real V1(3), V2(3), the two vectors.
%
%    Output, real V3(3), the cross product vector.
%
  v3 = zeros ( 3, 1 );

  v3(1) = v1(2) * v2(3) - v1(3) * v2(2);
  v3(2) = v1(3) * v2(1) - v1(1) * v2(3);
  v3(3) = v1(1) * v2(2) - v1(2) * v2(1);

  return
end


function H=haarmtx(n)

% HAARMTX Compute Haar orthogonal transform matrix.
%
%   H = HAARMTX(N) returns the N-by-N HAAR transform matrix.  H*A
%   is the HAAR transformation of the columns of A and H'*A is the inverse
%   transformation of the columns of A (when A is N-by-N).
%   If A is square, the two-dimensional Haar transformation of A can be computed
%   as H*A*H'. This computation is sometimes faster than using
%   DCT2, especially if you are computing large number of small
%   Haar transformation, because H needs to be determined only once.
%
%   Class Support
%   -------------
%   N is an integer scalar of class double. H is returned
%   as a matrix of class double.
%
%
%   I/O Spec
%   N - input must be double
%   D - output DCT transform matrix is double
%
%   Author : Frederic Chanal (f.j.chanal@student.tue.nl) - 2004

a=1/sqrt(n);
for i=1:n
    H(1,i)=a;
end
for k=1:n-1
    p=fix(log2(k));
    q=k-2^p+1;
    t1=n/2^p;
    sup=fix(q*t1);
    mid=fix(sup-t1/2);
    inft=fix(sup-t1);
    t2=2^(p/2)*a;
    for j=1:inft
        H(k+1,j)=0;
    end
    for j=inft+1:mid
        H(k+1,j)=t2;
    end
    for j=mid+1:sup
        H(k+1,j)=-t2;
    end
    for j=sup+1:n
        H(k+1,j)=0;
    end
end


function value = mono_upto_enum ( m, n )

%*****************************************************************************80
%
%% MONO_UPTO_ENUM enumerates monomials in M dimensions of degree up to N.
%
%  Discussion:
%
%    For M = 2, we have the following values:
%
%    N  VALUE
%
%    0    1
%    1    3
%    2    6
%    3   10
%    4   15
%    5   21
%
%    In particular, VALUE(2,3) = 10 because we have the 10 monomials:
%
%      1, x, y, x^2, xy, y^2, x^3, x^2y, xy^2, y^3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the maximum degree.
%
%    Output, integer VALUE, the number of monomials in D variables,
%    of total degree N or less.
%
  value = nchoosek ( n + m, n );

  return
end


function value = f1 ( x )

%*****************************************************************************80
%
%% F1 evaluates a function that can be used for Chebyshev interpolation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 September 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(), one or more points where the function is to be evaluated.
%
%    Output, real VALUE(), the function values.
%
  value = cos ( 2.0 * pi * x ) .* sin ( 3.0 * pi * x );

  return
end


function [Theta, k] = estimate_plane(X, s)

% [Theta k] = estimate_plane(X)
%
% DESC:
% estimate the parameters of a 3D plane given the pairs [x, y, z]^T
% Theta = [a; b; c; d] where:
%
% a*x1+b*y1+c*z1+d = 0
% a*x2+b*y2+c*z2+d = 0
% a*x3+b*y3+c*z3+d = 0
%
% AUTHOR
% Marco Zuliani - marco.zuliani@gmail.com
%
% VERSION:
% 1.0.0
%
% INPUT:
% X                 = 3D points
% s                 = indices of the points used to estimate the parameter
%                     vector. If empty all the points are used
%
% OUTPUT:
% Theta             = estimated parameter vector Theta = [a; b; c; d]
% k                 = dimension of the minimal subset

% HISTORY:
% 1.0.0             = 07/05/08 - initial version

% cardinality of the MSS
k = 3;

if (nargin == 0) || isempty(X)
    Theta = [];
    return;
end;

if (nargin == 2) && ~isempty(s)
    X = X(:, s);
end;

% check if we have enough points
N = size(X, 2);
if (N < k)
    error('estimate_plane:inputError', ...
        'At least 3 points are required');
end;

A = [transpose(X(1, :)) transpose(X(2, :)) transpose(X(3, :)) ones(N, 1)];
[U S V] = svd(A);
Theta = V(:, 4);

return;


function h_new = fd1d_heat_explicit ( x_num, x, t, dt, cfl, rhs, bc, h )

%*****************************************************************************80
%
%% FD1D_HEAT_EXPLICIT: Finite difference solution of 1D heat equation.
%
%  Discussion:
%
%    This program takes one time step to solve the 1D heat equation
%    with an explicit method.
%
%    This program solves
%
%      dUdT - k * d2UdX2 = F(X,T)
%
%    over the interval [A,B] with boundary conditions
%
%      U(A,T) = UA(T),
%      U(B,T) = UB(T),
%
%    over the time interval [T0,T1] with initial conditions
%
%      U(X,T0) = U0(X)
%
%    The code uses the finite difference method to approximate the
%    second derivative in space, and an explicit forward Euler approximation
%    to the first derivative in time.
%
%    The finite difference form can be written as
%
%      U(X,T+dt) - U(X,T)                  ( U(X-dx,T) - 2 U(X,T) + U(X+dx,T) )
%      ------------------  = F(X,T) + k *  ------------------------------------
%               dt                                   dx * dx
%
%    or, assuming we have solved for all values of U at time T, we have
%
%      U(X,T+dt) = U(X,T) + cfl * ( U(X-dx,T) - 2 U(X,T) + U(X+dx,T) ) + dt * F(X,T)
%
%    Here "cfl" is the Courant-Friedrichs-Loewy coefficient:
%
%      cfl = k * dt / dx / dx
%
%    In order for accurate results to be computed by this explicit method,
%    the CFL coefficient must be less than 0.5!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    George Lindfield, John Penny,
%    Numerical Methods Using MATLAB,
%    Second Edition,
%    Prentice Hall, 1999,
%    ISBN: 0-13-012641-1,
%    LC: QA297.P45.
%
%  Parameters:
%
%    Input, integer X_NUM, the number of points to use in the spatial dimension.
%
%    Input, real X(X_NUM,1), the coordinates of the nodes.
%
%    Input, real T, the current time.
%
%    Input, real DT, the size of the time step.
%
%    Input, real CFL, the Courant-Friedrichs-Loewy coefficient,
%    computed by FD1D_HEAT_EXPLICIT_CFL.
%
%    Input, real H(X_NUM,1), the solution at the current time.
%
%    Input, @RHS, the function which evaluates the right hand side.
%
%    Input, @BC, the function which evaluates the boundary conditions.
%
%    Output, real H_NEW(X_NUM,1), the solution at time T+DT.
%
  h_new = zeros(x_num,1);

  L = 1 : x_num - 2;
  C = 2 : x_num - 1;
  R = 3 : x_num;

  f = rhs ( x_num, x, t );

  h_new(C) = h(C) + cfl * ( h(L) - 2.0 * h(C) + h(R) ) + dt * f(C);

  h_new = bc ( x_num, x, t + dt, h_new );

  return
end


function [feven,fodd] = filterGabor1d( r, sig, omega, show )
% Creates an even/odd pair of 1D Gabor filters.
%
% USAGE
%  [feven,fodd] = filterGabor1d( r, sig, omega, [show] )
%
% INPUTS
%  r       - final filter will be 2r+1 (good choice for r is r=2*sig)
%  sig     - standard deviation of Gaussian mask
%  omega   - frequency of underlying sine/cosine in [1/(2r+1) r/(2r+1)]
%  show    - [0] figure to use for optional display
%
% OUTPUTS
%  feven   - even symmetric filter (-cosine masked with Gaussian)
%  fodd    - odd symmetric filter (-sine masked with Gaussian)
%
% EXAMPLE
%  sig = 15; f=filterGabor1d(2*sig,sig,1/sig,1);
%
% See also FILTERGABOR2D
%
% 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<4 || isempty(show) ); show=0; end;

r = ceil(r);  n=2*r+1;
if( omega<1/(n) || omega>r/(n) )
  error(['omega=' num2str(omega) ' out of range =[' num2str([1 r]/n) ']']);
end;

% create even and odd pair
x = -r:r;
feven = -cos(2*pi*x*omega) .* exp(-(x.^2)/sig^2);
fodd  = -sin(2*pi*x*omega) .* exp(-(x.^2)/sig^2); %=imag(hilbert(feven));

% normalize to mean==0, but only in locs that are nonzero
inds = abs(feven)>.00001;  feven(inds) = feven(inds) - mean(feven(inds));
inds = abs(fodd)>.00001;  fodd(inds) = fodd(inds) - mean(fodd(inds));

% set L1norm to 0
feven = feven/norm(feven(:),1);
fodd = fodd/norm(fodd(:),1);

% visualization
if( show )
  filterVisualize( feven, show );
  filterVisualize( fodd, show+1 );
end


function sig=sigmerge(x1,x2,ratio);
%SIGMERGE Add two signals with given energy ratio in dB.
%	SIG=SIGMERGE(X1,X2,RATIO) adds two signals so that a given
%	energy ratio expressed in deciBels is satisfied.
%
%	X1, X2 : input signals.
%	RATIO  : Energy ratio in deciBels	(default : 0 dB).
%	X      : output signal.
%	X= X1+H*X2, such that 10*log(Energy(X1)/Energy(H*X2))=RATIO
%
%	Example :
%	 sig=fmlin(64,0.01,0.05,1); noise=hilbert(randn(64,1));
%	 SNR=15; x=sigmerge(sig,noise,SNR);
%	 Esig=mean(abs(sig).^2); Enoise=mean(abs(x-sig).^2);
%	 10*log10(Esig/Enoise)

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

if (nargin<2)
 error('At least two parameters are required');
elseif nargin==2,
 ratio=0;
end;

[x1row,x1col] = size(x1);
[x2row,x2col] = size(x2);

if (x1col~=1)|(x2col~=1),
 error('X1 and X2 must have only one column');
elseif (x1row~=x2row),
 error('X1 and X2 must have the same number of rows');
elseif (length(ratio)~=1),
 error('RATIO must be a scalar');
elseif (ratio==inf),
 sig = x1;
else
 Ex1=mean(abs(x1).^2);
 Ex2=mean(abs(x2).^2);
 h=sqrt(Ex1/(Ex2*10^(ratio/10)));
 sig=x1+h*x2;
end;


function p2Vals=floorPow2(nums)
%%FLOORPOW2   Find the largest powers of 2 that are less than or equal
%             to the non-negative, real values in vals.
%
%INPUTS:  nums  An array or matrix of non-negative, real values.
%
%OUTPUTS:p2Vals Values corresponding to the power of 2 less than or
%               equal to the values in nums having the same datatype as
%               nums (float, int32, etc.).
%
%This function checks for the data type of the passed value. If the
%value passed returns true from the function isfloat then an algorithm
%that manipulates the mantissa and exponent of a floating point number is
%used. If the data type is not a float, then it is assumed to be an integer
%and binary shift operations are used to find the next highest power of
%two.
%
%The implementation here could be easily translated in C, since the
%floating point operations correspond to functions in math.h and the
%bitshift function for integer values just implements the << and >>
%operators in C.
%
%April 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

assert(all(nums>=0))

if(isfloat(nums))
    %log2 with two return values is the frexp function in C.
    [F,E]=log2(nums);

    %pow2 with two arguments is the ldexp function in C.
    p2Vals=pow2(0.5*(F>0),E);
else
    p2Vals=zeros(size(nums),'like',nums);
    numVals=size(nums(:));

    for curVal=1:numVals
        val=ones(1,1,'like',nums);
        temp=nums(curVal);

        if(temp==0)
            p2Vals(curVal)=0;
            continue;
        end

        while(temp~=1)
            temp=bitshift(temp,-1);
            val=bitshift(val,1);
        end

        p2Vals(curVal)=val;
    end
end
end

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


function theta = angle2Points(varargin)
%ANGLE2POINTS Compute horizontal angle between 2 points
%
%   ALPHA = angle2Points(P1, P2),
%   Pi are either [1*2] arrays, or [N*2] arrays, in this case ALPHA is a
%   [N*1] array. The angle computed is the horizontal angle of the line
%   (P1 P2)
%   Result is always given in radians, between 0 and 2*pi.
%
%   See Also:
%   points2d, angles2d, angle3points, normalizeAngle, vectorAngle
%
%
% ---------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% created the 02/03/2007.
% Copyright 2010 INRA - Cepia Software Platform.

%   HISTORY:
%   2011-01-11 use bsxfun

% process input arguments
if length(varargin)==2
    p1 = varargin{1};
    p2 = varargin{2};
elseif length(varargin)==1
    var = varargin{1};
    p1 = var(1,:);
    p2 = var(2,:);
end

% ensure data have correct size
n1 = size(p1, 1);
n2 = size(p2, 1);
if n1~=n2 && min(n1, n2)>1
    error('angle2Points: wrong size for inputs');
end

% angle of line (P2 P1), between 0 and 2*pi.
dp = bsxfun(@minus, p2, p1);
theta = mod(atan2(dp(:,2), dp(:,1)) + 2*pi, 2*pi);


function density = calc_packing_density(dim,N,min_euclidean_dist)
%CALC_PACKING_DENSITY Density of packing given by minimum distance
%
%Syntax
% density = calc_packing_density(dim,N,min_euclidean_dist);
%
%Description
% DENSITY = CALC_PACKING_DENSITY(dim,N,MIN_EUCLIDEAN_DIST) sets DENSITY to
% be the density of a packing of S^dim by N equal spherical caps with centers
% having minimum Euclidean distance MIN_EUCLIDEAN_DIST.
%
% The argument dim must be a positive integer.
% The argument N must be a positive integer or an array of positive integers.
% The argument MIN_EUCLIDEAN_DIST must be an array of real nubers of the same array size as N.
% The result DENSITY will be an array of the same size as N.
%
%Notes
% The packing density is defined to be the sum of the areas of the spherical
% caps of the packing, divided by the area of the unit sphere S^dim.
%
% The spherical radius of the caps in the packing is half the minimum spherical
% distance between center points. The spherical radius for N == 1 is a special
% case. It is defined to be pi.
%
%Examples
% > N=2:6
%  N =
%       2     3     4     5     6
%
% > dist=eq_min_dist(2,N)
%  dist =
%      2.0000    1.4142    1.4142    1.4142    1.4142
%
% > calc_packing_density(2,N,dist)
%  ans =
%      1.0000    0.4393    0.5858    0.7322    0.8787
%
%See also
% EQ_MIN_DIST, AREA_OF_CAP, AREA_OF_SPHERE, EQ_PACKING_DENSITY

% Copyright 2004-2005 Paul Leopardi for the University of New South Wales.
% $Revision 1.10 $ $Date 2005-06-01 $
% Documentation files renamed
% $Revision 1.10 $ $Date 2005-05-27 $
% Function e2s changed name to euc2sph_dist
% $Revision 1.00 $ $Date 2005-02-12 $
%
% For licensing, see COPYING.
% For references, see AUTHORS.
% For revision history, see CHANGELOG.

%
% Check number of arguments
%
error(nargchk(3,3,nargin));
%
s_cap = euc2sph_dist(min_euclidean_dist)/2;
%
% The spherical radius for N == 1 is a special case. It is pi.
%
s_cap(N == 1) = pi;
density = N.*area_of_cap(dim, s_cap)./area_of_sphere(dim);
%
% end function


% Derive_pwPoly5.m
%
% This script derives the equations that give the bezier coefficients for a
% curve, given the value, slope, and curvature at each endpoint.
%

clc; clear;

% Control Points:
n = 5;  %order of bezier curve
p = sym('p',[n+1,1]); assume(p,'real');

% Independent variable:
syms t 'real'

% Build up the equation for a 5th-order bezier curve:
x = sym(0);
for i=0:n
    tt = (t.^i).*(1-t).^(n-i);
    binom = nchoosek(n,i);
    x = x + binom*p(i+1)*tt;
end

% Now compute the derivaives:
dx = diff(x,t);
ddx = diff(dx,t);

% Evaluate at endpoints:
X0 = simplify(subs(x,t,0));
dX0 = simplify(subs(dx,t,0));
ddX0 = simplify(subs(ddx,t,0));
X1 = simplify(subs(x,t,1));
dX1 = simplify(subs(dx,t,1));
ddX1 = simplify(subs(ddx,t,1));

% User-Inputs:
syms x0 dx0 ddx0 x1 dx1 ddx1 'real'

% Constraint equations:
eqns = [...
    X0-x0;
    dX0-dx0;
    ddX0-ddx0;
    X1-x1;
    dX1-dx1;
    ddX1-ddx1];

% Solve for unknowns
[A,b] = equationsToMatrix(eqns,p);
pSoln = A\b;

% Substitute solution:
xSoln = simplify(subs(x,p,pSoln));
dxSoln = simplify(subs(dx,p,pSoln));
ddxSoln = simplify(subs(ddx,p,pSoln));

% Write solution to a matlab function:
matlabFunction(xSoln, dxSoln, ddxSoln,...
    'file','autoGen_pwPoly5.m',...
    'vars',{t,x0 dx0 ddx0 x1 dx1 ddx1},...
    'outputs',{'x','dx','ddx'});


function area = sphere_cap_area_3d ( r, h )

%*****************************************************************************80
%
%% SPHERE_CAP_AREA_3D computes the surface area of a spherical cap in 3D.
%
%  Discussion:
%
%    Draw any radius of the sphere and note the point P where the radius
%    intersects the sphere.  Consider the point on the radius line which is
%    H units from P.  Draw the circle that lies in the plane perpendicular to
%    the radius, and which intersects the sphere.  The circle divides the sphere
%    into two pieces, and the corresponding disk divides the solid sphere into
%    two pieces.  The spherical cap is the part of the solid sphere that
%    includes the point P.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the sphere.
%
%    Input, real H, the "height" of the spherical cap.
%    H must be between 0 and 2 * R.
%
%    Output, real AREA, the area of the spherical cap.
%
  if ( h <= 0.0 )
    area = 0.0;
  elseif ( 2.0 * r <= h )
    area = 4.0 * pi * r * r;
  else
    area = 2.0 * pi * r * h;
  end

  return
end


function prob_test078 ( )

%*****************************************************************************80
%
%% TEST078 tests GAMMA_MEAN, GAMMA_SAMPLE, GAMMA_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  test_num = 2;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST078\n' );
  fprintf ( 1, '  For the Gamma PDF:\n' );
  fprintf ( 1, '  GAMMA_MEAN computes the mean;\n' );
  fprintf ( 1, '  GAMMA_SAMPLE samples;\n' );
  fprintf ( 1, '  GAMMA_VARIANCE computes the variance.\n' );

  a_test(1:2) = [ 1.0, 2.0 ];
  b_test(1:2) = [ 3.0, 0.5 ];
  c_test(1:2) = [ 2.0, 0.5 ];

  for test_i = 1 : test_num

    a = a_test(test_i);
    b = b_test(test_i);
    c = c_test(test_i);

    check = gamma_check ( a, b, c );

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

    mean = gamma_mean ( a, b, c );
    variance = gamma_variance ( a, b, c );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  TEST NUMBER: %6d\n', test_i );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  PDF parameter A =             %14f\n', a );
    fprintf ( 1, '  PDF parameter B =             %14f\n', b );
    fprintf ( 1, '  PDF parameter C =             %14f\n', c );
    fprintf ( 1, '  PDF mean =                    %14f\n', mean );
    fprintf ( 1, '  PDF variance =                %14f\n', variance );

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

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

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

  end

  return
end


function result=L1median(x,tol);

%L1MEDIAN is an orthogonally equivariant location estimator,
% also known as the spatial median. It is defined as the point which
% minimizes the sum of the Euclidean distances to all observations in the
% data matrix x. It can resist 50% outliers.
%
% Reference (for the algorithm):
%    Hossjer, O. and Croux, C. (1995)
%    "Generalizing univariate signed rank statistics for testing and estimating
%    a multivariate location parameter", Nonparametric Statistics, 4, 293-308.
%
% Required input argument:
%    x : either a data matrix with n observations in rows, p variables in columns
%        or a vector of length n.
%
% Optional input argument:
%  tol : convergence criterium; the iterative process stops when the norm between two solutions < tol.
%        (default = 1.e-08).
%
% I/O: result=L1median(x,tol);
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Original Gauss code by C. Croux, translated to MATLAB by Sabine Verboven
% Last updated 18/02/2004

if nargin <2
   tol=1.e-08;
end;
[n,p]=size(x);
maxstep=200;
%initializing starting value for m
m=median(x);
k=1;
while (k<=maxstep)
   mold=m;
   Xext=sortrows([norme(x-repmat(m,n,1)) x],1);
   dx=Xext(:,1);
   x=Xext(:,2:p+1);
   if all(dx)
      w=1./dx;
   else
      ww=dx(all(dx,2));
      w=1./ww;
      w=[zeros(length(dx)-length(w),1);w];
   end
   delta=sum((x-repmat(m,n,1)).*repmat(w,1,p),1)./sum(w);
   nd=norme(delta);
   if all(nd<tol)
      maxhalf=0;
   else
      maxhalf=log2(nd/tol);
   end
   m=mold+delta;   %computation of a new estimate
   nstep=0;
   while all(mrobj(x,m)>=mrobj(x,mold))&(nstep<=maxhalf)
      nstep=nstep+1;
      m=mold+delta./(2^nstep);
   end
   if (nstep>maxhalf)
      mX=mold;
      break
   end
   k=k+1;
end
if k>maxstep
   display('Iteration failed')
end
result=m;

%------------------------------------------------------------------
function n=norme(x);

%NORME calculates the Euclidian norm of a matrix x
% the output is a column vector containing the norm of each row
% I/O: n=norme(X);

for i=1:size(x,1)
   n(i)=norm(x(i,:));
end;
n=n';

%------------------------------------------------------------------
function s=mrobj(x,m)

%MROBJ computes the objective function in m based on x and a

xm=norme(x-repmat(m,size(x,1),1));
s=sum(xm,1)';


function area = sphere_cap_area_3d ( r, h )

%*****************************************************************************80
%
%% SPHERE_CAP_AREA_3D computes the surface area of a spherical cap in 3D.
%
%  Discussion:
%
%    Draw any radius of the sphere and note the point P where the radius
%    intersects the sphere.  Consider the point on the radius line which is
%    H units from P.  Draw the circle that lies in the plane perpendicular to
%    the radius, and which intersects the sphere.  The circle divides the sphere
%    into two pieces, and the corresponding disk divides the solid sphere into
%    two pieces.  The spherical cap is the part of the solid sphere that
%    includes the point P.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 April 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the sphere.
%
%    Input, real H, the "height" of the spherical cap.
%
%    Output, real AREA, the area of the spherical cap.
%
  if ( h <= 0.0 )
    area = 0.0;
  elseif ( 2.0 * r <= h )
    area = 4.0 * pi * r * r;
  else
    area = 2.0 * pi * r * h;
  end

  return
end


function x = chebyshev1 ( n )

%*****************************************************************************80
%
%% CHEBYSHEV1 returns the Type 1 Chebyshev points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2018
%
%  Author:
%
%    John Burkardt.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), the points.
%
  x(1:n) = cos ( ( 2.0 * (0:n-1) + 1.0 ) * pi / 2 / n );

  return
end


% Derive_acrobot.m
%
% This script derives the equations of motion for the simple acrobot robot:
% a double pendulum with a motor at the elbow joint.
%

syms q1 q2 dq1 dq2 ddq1 ddq2 'real'   % states
syms u 'real' % actuation
syms m1 m2 g l1 l2 'real' % physical parameters

%%%% Unit vectors:
i = sym([1;0]);
j = sym([0;1]);

e1 = cos(q1)*(-j) + sin(q1)*(i);    % shoulder -> elbow
e2 = cos(q2)*(-j) + sin(q2)*(i);    % elbow -> wrist

%%%% State vectors:
z = [q1;q2;dq1;dq2];
dz = [dq1;dq2;ddq1;ddq2];

%%%% Kinematics:
p1 = l1*e1;
p2 = p1 + l2*e2;

dp1 = jacobian(p1,z)*dz;  %Chain rule to get velocity of elbow joint
dp2 = jacobian(p2,z)*dz;

ddp1 = jacobian(dp1,z)*dz;
ddp2 = jacobian(dp2,z)*dz;

%%%% Define a function for doing '2d' cross product: dot(a x b, k)
cross2d = @(a,b)(a(1)*b(2) - a(2)*b(1));

%%%% Angular momentum balance of system about shoulder joint:
sumTorques1 = cross2d(p1,-m1*g*j) + cross2d(p2,-m2*g*j);
sumInertial1 = cross2d(p1,m1*ddp1) + cross2d(p2,m2*ddp2);
eqn1 = sumTorques1-sumInertial1;

%%%% Angular momentum balance of outer link about elbow joint:
sumTorques2 = cross2d(p2-p1,-m2*g*j) + u;
sumInertial2 = cross2d(p2-p1,m2*ddp2);
eqn2 = sumTorques2-sumInertial2;

%%%% Write out dynamics in matrix form:   MM*ddq = ff
ddq = [ddq1;ddq2];
eqns = [eqn1;eqn2];
[MM,ff] = equationsToMatrix(eqns,ddq);

%%%% Generate an optimized matlab function for dynamics:
matlabFunction(MM(1,1),MM(1,2),MM(2,1),MM(2,2),ff(1),ff(2),G,B,...
    'file','autoGen_acrobotDynamics.m',...
    'vars',{q1,q2,dq1,dq2,u,m1,m2,g,l1,l2},...
    'outputs',{'M11','M12','M21','M22','f1','f2'});

%%%% Compute the energy of the system:
U = m1*g*dot(p1,j) + m2*g*dot(p2,j);   %Potential Energy
T = 0.5*m1*dot(dp1,dp1) + 0.5*m2*dot(dp2,dp2);   %Kinetic Energy

%%%% Generate an optimized matlab function for energy:
matlabFunction(U,T,...
    'file','autoGen_acrobotEnergy.m',...
    'vars',{q1,q2,dq1,dq2,m1,m2,g,l1,l2},...
    'outputs',{'U','T'});

%%%% Generate a function for computing the kinematics:
matlabFunction(p1,p2,dp1,dp2,...
    'file','autoGen_acrobotKinematics.m',...
    'vars',{q1,q2,dq1,dq2,l1,l2},...
    'outputs',{'p1','p2','dp1','dp2'});


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% polint1.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [g,G] = polint1(x,f)
% quadratic polynomial interpolation
% Input:
% x(1:3)  3 pairwise distinct support points
% f(1:3)  corresponding function values
% Output:
% g, G
% the interpolating polynomial is given by
% p(x) = f(1) + g(x - x(1)) + G/2(x - x(1))^2

function [g,G] = polint1(x,f)
f13 = (f(3)-f(1))/(x(3)-x(1));
f12 = (f(2)-f(1))/(x(2)-x(1));
f23 = (f(3)-f(2))/(x(3)-x(2));
g = f13 + f12 - f23;
G = 2*(f13-f12)/(x(3)-x(2));


%% 2D Laplace Equation
% This program solves the following PDE:
%
% $$\frac{\partial^2{T}}{\partial{x^2}}+\frac{\partial^2{T}}{\partial{y^2}}=0$$

clear
clc
close all

%% Discretization of space
% Length and heigth of our 2D domain:
L = 1; % length in x direction
H = 1; % height in y direction

% our target is to evaluate the step h size, thus
h = 0.05; % we asume and equally spaced XY grid
hx = h;
hy = h;

% number of points
n = round(L/hx);
m = round(H/hy);

% initaliza Matriz T
T=zeros(n+1,m+1);

%% Boundary Contidions
% Our domain requires 4 boudary conditions in total, can be only nuemann,
% or only dirichlet or a combination of both.

%% Nuemann Boundary Conditions (Prescribed values at the borders)
% Bottom (last row of the matrix)
T(1,:)=0;
% Top (first row of the matrix)
%T(1,:)=1;
for i=0:n
    %using function of x to prescribe the values.
    T(m+1,i+1)=1-cos(pi/(n)*i);
end
% Left
%T(:,1)=0;
% Right
%T(:,n+1)=2;

%T

% Vector of Prescribed Bondary Values
% Bottom
fb=(T(1,2:n)).' ; %transpose this array
% Top
ft=T(m+1,2:n).' ; %transpose this array
% Left
fl=T(2:n,1);
% Right
fr=T(2:n,n+1);

% Computing vector of known values
fs=zeros(n-1,1);
fs(1)=fl(1);
fs(n-1)=fr(1);
w=fb+fs;
for i=2:n-2
    fs(1)=fl(i);
    fs(n-1)=fr(i);
    w=cat(1,w,fs);
end
fs=zeros(n-1,1);
fs(1)=fl(n-1);
fs(n-1)=fr(n-1);
y=ft+fs;
w=cat(1,w,y);

%% Dirichlect Boundary Conditions (flux at the borders of the domain)
% This conditions requires de mofification of the diagonal values of our
% solution matrix:

% Bottom (last row of the matrix)
%sb=ones(n-1,1); % to insulate this boundary
sb=zeros(n-1,1); %to suppres this condition
% Top (first row of the matrix)
%st=ones(n-1,1); % to insulate this boundary
st=zeros(n-1,1); %to suppres this condition
% Left
sl=zeros(m-1,1);
sl(1)=1; % to insulate this boundary
% Right
sr=zeros(m-1,1);
sr(m-1)=1; % to insulate this boundary

% Computing the diagonal matrix to substract
s=sl+sr; %sum condition in the right and left
stt=st+s; %sum condition of the top and sides
sbb=sb+s; %sum condition of the bottom and sides
u=cat(1,s,s);
for i=4:n-2
    u=cat(1,u,s);
end
z=cat(1,stt,u);
z=cat(1,z,sbb);
Z=spdiags(z,0,length(z),length(z)); %transforms the vector in a diagonal mat.
%spy(Z)

%% System Solution
% Our solution has the shape:
%
% $$ A_{ij}u_j=f_i  $$

% Formulating A:
I=eye(m-1);
e=ones(m-1,1);
K=spdiags([e,-4*e,e],[-1,0,1],m-1,m-1);
S=spdiags([e,e],[-1,1],m-1,m-1);
A=(kron(I,K)+kron(S,I));
A=(A+Z); %(for flux conditions)
u=inv(A)*(-w);
spy(A)

%% Re-arranging the data in vector u

for j=1:n-1
    for i=1:n-1
        t(j,i)=u(i+(n-1)*(j-1));
    end
end
for j=1:n-1
    for i=1:n-1
        T(i+1,j+1)=t(i,j);
    end
end

% Make up for the plot
if sum(sb)>=1
    T(1,:)=T(2,:);
end
if sum(st)>=1
    T(n+1,:)=T(n,:);
end
if sum(sl)>=1
    T(:,1)=T(:,2);
end
if sum(sr)>=1
    T(:,n+1)=T(:,n);
end
%T
%surf(T)
contourf(T)
colormap hot
colorbar('location','southoutside')

%% Computing the Error of our aproximation
% % The exact solution of our problem is given by:
% %
% % $$ T(x,y)=y-\frac{sinh(\pi y))}{sinh(\pi)}cos(\pi x) $$
% for j=1:m+1
%     for i=1:n+1
%         T2(j,i)=j/(m+1)-sinh(pi/(m+1)*j)/sinh(pi)*cos(pi/(n+1)*i);
%     end
% end
% Error=T2-T;
% %T2
% h
% NormError=norm(Error)
% %surf(T2)
% contourf(T2)
% colormap hot
% colorbar('location','southoutside')


function a = borderband ( n )

%*****************************************************************************80
%
%% BORDERBAND returns the BORDERBAND matrix.
%
%  Formula:
%
%    If ( I = J )
%      A(I,I) = 1
%    else if ( I = N )
%      A(N,J) = 2^(1-J)
%    else if ( J = N )
%      A(I,N) = 2^(1-I)
%    else
%      A(I,J) = 0
%
%  Example:
%
%    N = 5
%
%     1  0   0   0   1
%     0  1   0   0  1/2
%     0  0   1   0  1/4
%     0  0   0   1  1/8
%     1 1/2 1/4 1/8  1
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    A is border-banded.
%
%    A has N-2 eigenvalues of 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n

    for j = 1 : n

      if ( i == j )
        a(i,j) = 1.0;
      elseif ( j == n )
        a(i,j) = 2.0^(1-i);
      elseif ( i == n )
        a(i,j) = 2.0^(1-j);
      else
        a(i,j) = 0.0;
      end

    end

  end

  return
end


function alpha = anglePoints3d(varargin)
%ANGLEPOINTS3D Compute angle between three 3D points
%
%   ALPHA = anglePoints3d(P1, P2)
%   Computes angle (P1, O, P2), in radians, between 0 and PI.
%
%   ALPHA = anglePoints3d(P1, P2, P3)
%   Computes angle (P1, P2, P3), in radians, between 0 and PI.
%
%   ALPHA = anglePoints3d(PTS)
%   PTS is a 3x3 or 2x3 array containing coordinate of points.
%
%   See also
%   points3d, angles3d
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 21/02/2005.
%

%   HISTORY
%   20/09/2005: add case of single argument for all points
%   04/01/2007: check typo
%   27/05/2014: adjust known vector sizes n1, n0, n2 once corrected for

p2 = [0 0 0];
if length(varargin) == 1
    pts = varargin{1};
    if size(pts, 1)==2
        p1 = pts(1,:);
        p0 = [0 0 0];
        p2 = pts(2,:);
    else
        p1 = pts(1,:);
        p0 = pts(2,:);
        p2 = pts(3,:);
    end

elseif length(varargin) == 2
    p1 = varargin{1};
    p0 = [0 0 0];
    p2 = varargin{2};

elseif length(varargin) == 3
    p1 = varargin{1};
    p0 = varargin{2};
    p2 = varargin{3};
end

% ensure all data have same size
n1 = size(p1, 1);
n2 = size(p2, 1);
n0 = size(p0, 1);

if n1 ~= n0
    if n1 == 1
        p1 = repmat(p1, [n0 1]);
        n1 = n0;
    elseif n0==1
        p0 = repmat(p0, [n1 1]);
    else
        error('Arguments P1 and P0 must have the same size');
    end
end

if n1 ~= n2
    if n1 == 1
        p1 = repmat(p1, [n2 1]);
    elseif n2 == 1
        p2 = repmat(p2, [n1 1]);
    else
        error('Arguments P1 and P2 must have the same size');
    end
end

% normalized vectors
p1 = normalizeVector3d(p1 - p0);
p2 = normalizeVector3d(p2 - p0);

% compute angle
alpha = acos(dot(p1, p2, 2));


%% DEMOHESSIAN  Short demonstration of Hessians
%

%% Some sample applications of the Hessian toolbox
% Hessians implement second order automatic differentiation in forward mode, which is
% conveniently to implement using the Matlab operator concept.
%

format compact long infsup
setround(0)                           % set rounding to nearest

%% Initialization of Hessians
% The initialization follows the same principles as for gradients, for example

x = hessianinit([ -.3 ; pi ])

%% Access of the Hessian
% Define the function f: R^n->R^n by

f = @(x)( 3*x(1)*x + sin(x).*(sec(x)-sqrt(x)) )

%%
% The number of unknowns is determined by the length of the input vector x.
% For example,

f(1:4)

%%
% The function can be evaluated using the Hessian package as follows:

y = f(hessianinit([1.1 -2.7 pi]'));

%%
% There is direct access of the Hessian of y=f(x) by

y.hx

%%
% However, y.hx contains the Hessians of all three component functions of
% the original function f. To access the Hessians it is recommended to
% use the Hessian of individual components, not components of y.hx:

H3 = y(3).hx

%%
% The matrix H3 is the Hessian of the third component function of f at x.

%% A simple example
% Consider the following function

f = inline(' sin(x-log(x+2))-x*cosh(x)/15 ')

%%
% To plot the function first vectorize f :

F = vectorize(f)

%%
% Plot the function including the x-axis:

x = linspace(-1,3); close; plot( x,F(x), x,0*x )

%% Zeros of a function
% There are two roots near 1.5 and 2.5, and two extrema
% near -0.5 and 2. The roots can be included by verifynlss.
% For this simple function the inclusion is of maximum accuracy.

X1 = verifynlss(f,1.5)
X2 = verifynlss(f,2.5)

%% Extrema of a function
% The extrema can be approximated and included using Hessians.
% The following is one step of a simple Newton iteration starting at x=-0.5 :

x = -0.5;
y = f(hessianinit(x));
x = x - y.hx\y.dx';
y

%% Inclusion of extrema
% Inclusions of the extrema of f are computed by "verifynlss" with
% parameter 'h': This parameter specifies that
% f'(x) = 0 is solved instead of f(x) = 0.

X1 = verifynlss(f,-0.5,'h')
X2 = verifynlss(f,2,'h')

%% Functions in several unknowns
% Function with several unknowns are handled in a similar manner.
% Consider the following test function by N. Gould. It is taken from
% the Coconut collection of test function for global optimization,
% http://www.mat.univie.ac.at/~neum/glopt/coconut/benchmark/Library2.html .

f = inline(' x(3)-1 + sqr(x(1)) + sqr(x(2)) + sqr(x(3)+x(4)) + sqr(sin(x(3))) +  sqr(x(1))*sqr(x(2)) + x(4)-3 + sqr(sin(x(3))) + sqr(x(4)-1) + sqr(sqr(x(2))) + sqr(sqr(x(3)) + sqr(x(4)+x(1))) + sqr(x(1)-4 + sqr(sin(x(4))) + sqr(x(2))*sqr(x(3))) + sqr(sqr(sin(x(4)))) ')

%% Approximation of an extremum
% On the Web-site the global minimum of that function in 4 unknowns is
% given as 5.7444 . We use a couple of a Newton iterations
% starting at x=ones(4,1) to approximate a stationary point:

format short
x = ones(4,1);
for i=1:18
   y = f(hessianinit(x));
   x = x - y.hx\y.dx';
end
y.dx

%%
% Now x is an approximation of a stationary point:
% The gradient of f evaluated at x is not too far from zero.

%% Inclusion of a stationary point
% Using this approximation an inclusion of a stationary point of f is
% computed by (in this case the last parameter 1 is used to see
% intermediate results):

format long
X = verifynlss(f,x,'h',1)

%% Inclusion of a minimum
% The interval vector X includes a root of f', i.e. a stationary point xx of f. To prove
% that f has a minumum at xx we need to prove positive definiteness of
% the Hessian of f evaluated at xx. The interval vector X includes the stationary point
% xx of f. So we compute an inclusion Y of the Hessian at X.
%
% Mathematically,
% for every x in X the following is true: Y.x is an inclusion of f(x),
% Y.dx is an inclusion of f'(x), and Y.hx is an inclusion of the
% Hessian of f at x. Especially, the Hessian of f evaluated at xx is
% included in Y.hx.

Y = f(hessianinit(X));
format _
Y.hx

%%
% This interval matrix contains obviously only diagonally dominant
% matrices, so the stationary point xx of f in X is indeed a (local)
% minimum.
%

%% A model problem in 5000 unknowns I
% The next problem is taken from
%
% http://www.sor.princeton.edu/~rvdb/ampl/nlmodels/cute/bdqrtic.mod
%
%    Source: Problem 61 in
%       A.R. Conn, N.I.M. Gould, M. Lescrenier and Ph.L. Toint,
%       "Performance of a multifrontal scheme for partially separable optimization",
%        Report 88/4, Dept of Mathematics, FUNDP (Namur, B), 1988.
%        Copyright (C) 2001 Princeton University
%        All Rights Reserved
%    see bottom of file test_h.m
%
% The model problem is
%
%     N = length(x);      % model problem: N = 1000, initial approximation x=ones(N,1);
%     I = 1:N-4;
%     y = sum( (-4*x(I)+3.0).^2 ) + sum( ( x(I).^2 + 2*x(I+1).^2 + ...
%               3*x(I+2).^2 + 4*x(I+3).^2 + 5*x(N).^2 ).^2 );
%
% This function is evaluated by
%
%     index = 2;
%     y = test_h(x,index);

%% A model problem in 5000 unknowns II
% The given starting vector is x = ones(5000,1).
% Recall that y = f(hessianinit(x)) has 5000 elements in y.x, 2.5e7 elements
% in y.dx and 1.25e11 elements in y.hx. In full storage this would mean 1 TeraByte
% of storage.
%
% The problem can be solved in the above manner. On my 2.8 GHz
% Laptop this requires about 1 second per Newton iteration, and 5 seconds
% for one function evaluation with interval argument.
%
% The following calculates an inclusion of a stationary point of f by first
% performing a simple Newton iteration followed by a verification step for
% the nonlinear system. The Hessian is treated as full matrix, so the
% computation may take a while.

n = 5000;
index = 2;
tic
X = verifynlss('test_h',ones(n,1),'h',1,index);
tfull = toc
max(relerr(X))

%% A model problem in 5000 unknowns III
% Note the small maximum relative error of the inclusion of the result.
% Verification is faster when solving the nonlinear system treating the
% Hessian as sparse. This is done by the following.

n = 5000;
index = 2;
tic
X = verifynlss('test_h',ones(n,1),'hSparseSPD',1,index);
tsparse = toc
tfull
max(relerr(X))

%%
% Note that verification is faster at the price of a less narrow inclusion
% (for comparison the previous time tfull is displayed).
%

%% Verification of a minimum
% Finally, the Hessian at X is computed which includes the Hessian at the
% stationary point xhat in X.

y = test_h(hessianinit(X),index);
isspd(y.hx)

%%
% The interval Hessian is proved by "isspd" to be symmetric
% positive definite: Mathematically the result "isspd(M)=1" for a symmetric
% (Hermitian) interval matrix M proves that every symmetric (Hermitian) matrix
% A within M is positive definite. In our case especially the Hermitian of f
% at the stationary point xhat. Therefore, xhat is proved to be a local minimum of f.

%% Enjoy INTLAB
% INTLAB was designed and written by S.M. Rump, head of the Institute for Reliable Computing,
% Hamburg University of Technology. Suggestions are always welcome to rump (at) tuhh.de


function Z = projectData(X, U, K)
%PROJECTDATA Computes the reduced data representation when projecting only
%on to the top k eigenvectors
%   Z = projectData(X, U, K) computes the projection of
%   the normalized inputs X into the reduced dimensional space spanned by
%   the first K columns of U. It returns the projected examples in Z.
%

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

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the projection of the data using only the top K
%               eigenvectors in U (first K columns).
%               For the i-th example X(i,:), the projection on to the k-th
%               eigenvector is given as follows:
%                    x = X(i, :)';
%                    projection_k = x' * U(:, k);
%
U_reduce = U(:, 1:K);
x = X';
Z = x' * U_reduce;

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

end


function variance = uniform_variance ( a, b )

%*****************************************************************************80
%
%% UNIFORM_VARIANCE returns the variance of the Uniform PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    A < B.
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = ( b - a )^2 / 12.0;

  return
end


%   Generic rank-r Newton's iteration routine solving    F(z) = 0  where
%   F is a smooth mapping between vector spaces, assuming the Jacobian
%   is of rank r at the solution that can be non-isolated
%   (c.f. http://homepages.neiu.edu/~zzeng/Papers/Rank-r_Newton.pdf)
%
%     Syntax
%         [z,res,fcond] = Newton({@F,domain,para}, @G, z0)
%         [z,res,fcond] = Newton({@F,domain,para}, @G, z0, trac)
%         [z,res,fcond] = Newton({@F,domain,para}, @G, z0, trac, tol)
%         [z,res,fcond] = Newton({@F,domain,para}, {@G,r}, z0)
%         [z,res,fcond] = Newton({@F,domain,para}, {@G,r}, z0, trac)
%         [z,res,fcond] = Newton({@F,domain,para}, {@G,r}, z0, trac, tol)
%
%     Input:   F -- (function) Matlab function name for calculating y = F(z)
%                    with syntax >> y = F(z1,...zk,p1,...,pm) where z1,...,zk
%                    are components of z and p1,...,pm are fixed parameters.
%                    (F must run on F(domain{:},para{:}).
%         domain -- domain of the homomorphism represented by
%                        (i) an mxn matrix of 0 'and 1's representing
%                            0 entries and variable entries respectively, or
%                       (ii) a polynomial with variable coefficients being
%                              nonzero, or
%                      (iii) a cell array of matrices in (i) and/or (ii)
%                      assuming the domain is a product of matrix spaces and
%                      polynomial spaces
%            para -- (cell) parameters needed for running F
%               G -- (function) Matlab function name for calculating the
%                     Jacobian u = F_z(z0)y F as a homomorphism with syntax
%                        >> u = G(y1,...,yk,z1,...zk,p1,...,pm)
%                     where z1,...,zk,p1,...,pk are the same for F but
%                     considered fixed, the variables are y1,...,yk in the
%                     same domain of F.
%               r -- (integer, optional) if provided, the rank-r projection
%                       of the Jacobian is used for the iteration
%              z0 -- (cell/matrix/polynomial) the initial iterate for the
%                    Newton's iteration in the domain
%            trac -- (0,1,or 2, optional) flag for tracking the iteration
%                     if tracking = 0 or missing, no tracking
%                     if tracking = 1, track the first component z1 of z
%                     if tracking = 2, track all components of z
%              tol -- (numeric, optional) error tolerance of the iterate
%                     if missing, iteration stops when residule stops
%                     decreasing
%
%   Output:         z  -- the least squares solution in the domain
%                 res  -- (optional) the residual ||F(z)||
%               fcond  -- (optional) the condition number


function q = sphere_stereograph ( p )

%*****************************************************************************80
%
%% SPHERE_STEREOGRAPH computes the stereographic image of points on a sphere.
%
%  Discussion:
%
%    We start with a sphere of radius 1 and center (0,0,0).
%
%    The north pole N = (0,0,1) is the point of tangency to the sphere
%    of a plane, and the south pole S = (0,0,-1) is the focus for the
%    stereographic projection.
%
%    For any point P on the sphere, the stereographic projection Q of the
%    point is defined by drawing the line from S through P, and computing
%    Q as the intersection of this line with the plane.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 November 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    C F Marcus,
%    The stereographic projection in vector notation,
%    Mathematics Magazine,
%    Volume 39, Number 2, March 1966, pages 100-102.
%
%  Parameters:
%
%    Input, real P(3,N), a set of points on the unit sphere.
%
%    Output, real Q(3,N), the coordinates of the image points,
%
  n = size ( p, 2 );
  q = zeros ( 3, n );

  q(1,1:n) =  2.0 * p(1,1:n) ./ ( 1.0 + p(3,1:n) );
  q(2,1:n) =  2.0 * p(2,1:n) ./ ( 1.0 + p(3,1:n) );
  q(3,1:n) =  1.0;

  return
end


function pde = sincosNeumanndata
%% SINCOSNEUMANNDATA trigonometric  data for Poisson equation
%
%     f = 8*pi^2*sin(2*pi*x)*cos(2*pi*y);
%     u = sin(2*pi*x)*cos(2*pi*y);
%     Du = (2*pi*cos(2*pi*x)*cos(2*pi*y), -2*pi*sin(2*pi*x)*sin(2*pi*y));
%     du/dn = g_N on [0,1]^2.
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

pde = struct('f',@f,'exactu',@exactu,'Du',@Du,'g_D',@g_D,'g_N',@g_N);

% load data (right hand side function)
function rhs =  f(p)
    x = p(:,1); y = p(:,2);
    rhs =  8*pi^2*sin(2*pi*x).*cos(2*pi*y);
end
% exact solution
function u =  exactu(p)
    x = p(:,1); y = p(:,2);
    u =  sin(2*pi*x).*cos(2*pi*y);
end
% derivative of the exact solution
function uprime =  Du(p)
    x = p(:,1); y = p(:,2);
    uprime(:,1) = 2*pi*cos(2*pi*x).*cos(2*pi*y);
    uprime(:,2) = -2*pi*sin(2*pi*x).*sin(2*pi*y);
end
function u = g_D(p)
    u = exactu(p);
end
% Neumann boundary condition
function f = g_N(p,vargin)
    if nargin > 1
        f = dot(Du(p),vargin,2);
    else
        f = zeros(size(p,1),1);
        x = p(:,1); y = p(:,2);
        uprime = [2*pi*cos(2*pi*x).*cos(2*pi*y) -2*pi*sin(2*pi*x).*sin(2*pi*y)];
        leftbd = (abs(x)<eps);  % n = (-1,0);
        f(leftbd) = - uprime(leftbd,1);
        rightbd = (abs(x-1)<eps); % n = (1,0);
        f(rightbd) = uprime(rightbd,1);
        topbd = (abs(y-1)<eps);   % n = (0,1)
        f(topbd) = uprime(topbd,2);
        bottombd = (abs(y)<eps);% n = (0,-1)
        f(bottombd) = - uprime(bottombd,2);
    end
end
end


function t=tquant(n, p)
%TQUANT  Quantiles of Student's t distribution
%
%  TQUANT(n, p) is the p-quantile of a t distributed random variable
%  with n degrees of freedom; that is, TQUANT(n, p) is the value below
%  which 100p percent of the t distribution with n degrees of freedom
%  lies.

%  Modified 13-Jul-06
%  Author: Tapio Schneider
%          tapio@gps.caltech.edu

%  References:
%  L. Devroye, 1986: "Non-Uniform Random Variate Generation", Springer
%
%  M. Abramowitz and I. A. Stegun, 1964: "Handbook of Mathematical
%     Functions"
%
%  See also: tcdf.m in the Matlab Statistics Toolbox (evaluates
%     cumulative distribution function of Student's t)

  if (n ~= round(n) | n < 1)
    error('Usage: TQUANT(n,p) - Degrees of freedom n must be positive integer.')
  end

  if (p<0 | p>1)
    error('Usage: TQUANT(n,p) - Probability p must be in [0,1].')
  elseif p == 1
    t   = Inf;
    return
  elseif p == 0
    t   = -Inf;
    return
  end

  if n == 1
    % Cauchy distribution (cf. Devroye [1986, pp. 29 and 450])
    t   = tan(pi*(p-.5));
  elseif p >= 0.5
    % positive t-values (cf. M. Abramowitz and I. A. Stegun [1964,
    % Chapter 26])
    b0  = [0, 1];
    f   = inline('1 - betainc(b, n/2, .5)/2 - p', 'b', 'n', 'p');
    opt = optimset('Display', 'off');
    b   = fzero(f, b0, opt, n, p);
    % old calling sequence (on Windows/Mac with older Matlab versions):
    %b   = fzero(f, b0, eps, 0, n, p);

    t   = sqrt(n/b-n);
  else
    % negative t-values
    b0  = [0, 1];
    f   = inline('betainc(b, n/2, .5)/2 - p', 'b', 'n', 'p');
    %opt = optimset('Display', 'off'); % does not work on Windows/Mac
    %b   = fzero(f, b0, opt, n, p);
    % old calling sequence (for Windows/Mac compatibility):
    b   = fzero(f, b0, eps, 0, n, p);
    t   = -sqrt(n/b-n);
  end


function runpenl(w0)

% runpenl(w0)
% See Article 2.5
% This example integrates the pendulum equation
%   theta"(t) + 0.1*theta'(t)+sin(theta) = 0
% and animates the motion

% The equation of motion of a pendulum is
% described by two first order equations:
% theta'(t)=w; w'(t)=-.1*w-sin(theta). Let
% z=[theta; w], then
% z'(t)=[z(2); 0.2*z(2)-sin(z(1))]

% Create a function defining the differential
% equation in matrix form

% pendulum=inline('[z(2);-sin(z(1))]','t','z');
pendulum=inline(...
   '[z(2);-0.2*z(2)-sin(z(1))]','t','z');

% Choose time limits for the solution
tmax=30; n=501; t=linspace(0,tmax,n);

% Set integration tolerances
opts=odeset('reltol',1e-5,'abstol',1e-5);

% Input angular velocity repeatedly
if nargin==0
  disp(' ')
  disp('   DAMPED PENDULUM MOTION DESCRIBED BY')
  disp(' theta"(t)+0.1*theta''(t)+sin(theta) = 0')
  while 1
    disp(' ')
    disp('Select the angular velocity at the lowest')
    disp('point. Values over 2.42 push the pendulum')
    disp('over the top. Input w0 (or press return')
    disp('to stop)')
    w0=input('w0 = ? > ');
    if isempty(w0)
      disp(' '), disp('All Done'), disp(' '), return
    end

    % Specify the initial conditions
    theta0=0; z0=[theta0;w0];

    % Solve the differential equation using ode45
    [t,th]=ode45(pendulum,t,z0,opts);

    % Plot the angular deflection of the pendulum
    plot(t,180/pi*th(:,1)), xlabel('time')
    ylabel('angular deflection (degrees)')
    title('ANGULAR DEFLECTION OF THE PENDULUM')
    disp(' ')
    disp('Press return to see the animation')
    grid on, shg, pause

    % Animate the motion
    for j=1:n
      T=th(j,1); X=[0;sin(T)]; Y=[0;-cos(T)];
      plot(X,Y,X(2),Y(2),'o','markersize',12)
      axis([-1,1,-1,1]), axis square, axis off
      drawnow, shg, pause(.05)
    end
    clf
  end
  return
else
  theta0=0; z0=[theta0;w0];
  [t,th]=ode45(pendulum,t,z0,opts);
  if max(th(:,1))>pi
     titl='OVER THE TOP';
  else
     titl='LARGE AMPLITUDE MOTION';
  end

  for j=1:n
     T=th(j,1); X=[0;sin(T)]; Y=[0;-cos(T)];
     plot(X,Y,X(2),Y(2),'o','markersize',12)
     axis([-1,1,-1,1]), axis square, axis off
     title(titl), drawnow, shg
  end
  pause(1), close, return
end


function [est, x, k] = pnorm(A, p, tol, prnt)
%PNORM   Estimate of matrix p-norm (1 <= p <= inf).
%        [EST, x, k] = PNORM(A, p, TOL) estimates the Holder p-norm of a
%        matrix A, using the p-norm power method with a specially
%        chosen starting vector.
%        TOL is a relative convergence tolerance (default 1E-4).
%        Returned are the norm estimate EST (which is a lower bound for the
%        exact p-norm), the corresponding approximate maximizing vector x,
%        and the number of power method iterations k.
%        A nonzero fourth input argument causes trace output to the screen.
%        If A is a vector, this routine simply returns NORM(A, p).
%
%        See also NORM, NORMEST, NORMEST1.

%        Note: The estimate is exact for p = 1, but is not always exact for
%        p = 2 or p = inf.  Code could be added to treat p = 2 and p = inf
%        separately.
%
%        Calls DUAL.
%
%        Reference:
%        N. J. Higham, Estimating the matrix p-norm, Numer. Math.,
%             62 (1992), pp. 539-555.
%        N. J. Higham, Accuracy and Stability of Numerical Algorithms,
%           Second edition, Society for Industrial and Applied Mathematics,
%           Philadelphia, PA, 2002; sec. 15.2.

if nargin < 2, error('Must specify norm via second parameter.'), end
[m,n] = size(A);
if min(m,n) == 1, est = norm(A,p); return, end

if nargin < 4, prnt = 0; end
if nargin < 3 | isempty(tol), tol = 1e-4; end

% Stage I.  Use Algorithm OSE to get starting vector x for power method.
% Form y = B*x, at each stage choosing x(k) = c and scaling previous
% x(k+1:n) by s, where norm([c s],p)=1.

sm = 9;  % Number of samples.
y = zeros(m,1); x = zeros(n,1);

for k=1:n

    if k == 1
       c = 1; s = 0;
    else
       W = [A(:,k) y];

       if p == 2   % Special case.  Solve exactly for 2-norm.
          [U,S,V] = svd(full(W));
          c = V(1,1); s = V(2,1);

       else

          fopt = 0;
          for th=linspace(0,pi,sm)
              c1 = cos(th); s1 = sin(th);
              nrm = norm([c1 s1],p);
              c1 = c1/nrm; s1 = s1/nrm;   % [c1 s1] has unit p-norm.
              f = norm( W*[c1 s1]', p );
              if f > fopt
                 fopt = f;
                 c = c1; s = s1;
              end
          end

       end
    end

    x(k) = c;
    y = x(k)*A(:,k) + s*y;
    if k > 1, x(1:k-1) = s*x(1:k-1); end

end

est = norm(y,p);
if prnt, fprintf('Alg OSE: %9.4e\n', est), end

% Stage II.  Apply Algorithm PM (the power method).

q = dual(p);
k = 1;

while 1

    y = A*x;
    est_old = est;
    est = norm(y,p);

    z = A' * dual(y,p);

    if prnt
        fprintf('%2.0f: norm(y) = %9.4e,  norm(z) = %9.4e', ...
                 k, norm(y,p), norm(z,q))
        fprintf('  rel_incr(est) = %9.4e\n', (est-est_old)/est)
    end

    if ( norm(z,q) <= z'*x | abs(est-est_old)/est <= tol ) & k > 1
       return
    end

    x = dual(z,q);
    k = k + 1;

end


classdef givens

methods(Static)

function [c,s] = rotation(a,b)
    % Givens rotation computation
    % Determines cosine-sine pair (c,s) so that [c s;-s c]'*[a;b] = [r;0]
    % G = [c  s; -s c]
    % x = [a; b]
    % G' x = [r; 0]
    % GVL4: algorithm 5.1.3
    if b==0
        % No rotation needed
        c = 1; s = 0;
    else
        if abs(b)>abs(a)
            tau = -a/b;
            s = 1/sqrt(1+tau^2);
            c = s*tau;
        else
            tau = -b/a;
            c = 1/sqrt(1+tau^2);
            s = c*tau;
        end
    end
end % function

function G = planerot(x)
    % Mimics the planerot function of MATLAB
    a = x(1);
    b = x(2);
    if b==0
        % No rotation needed
        c = 1; s = 0;
    else
        sa = sign(a);
        sb = sign(b);
        if abs(b)>abs(a)
            tau = -a/b;
            s = -sb/sqrt(1+tau^2);
            c = s*tau;
        else
            tau = -b/a;
            c = sa/sqrt(1+tau^2);
            s = c*tau;
        end
    end
    G = [c, -s; s c];
end % function

function theta = theta(c, s)
    theta = atan(s/c);
end

function [Q, R] = qr(A)
    % Givens method for computing the QR factorization A = QR.
    % A is modified in the algorithm
    % GVL4: algorithm 5.2.4
    import spx.la.givens.rotation;
    [m,n] = size(A);
    % Space for saving the Q factor
    Q = eye(m,m);
    % Iterate over columns from first to last
    for j=1:n
        % Iterate over rows from last pair to first pair
        for i=m:-1:j+1
            % pick two elements from consecutive rows
            a = A(i-1,j);
            b = A(i,j);
            % Compute the needed rotation for making b 0.
            [c,s] = rotation(a,b);
            % form the Givens rotation matrix
            G = [c s;-s c];
            % Rotate the two consecutive rows based submatrix
            A(i-1:i,j:n) = G'*A(i-1:i,j:n);
            % Rotate corresponding columns in Q matrix
            % by postmultiplication with G
            Q(:,i-1:i) = Q(:,i-1:i)*G;
        end
    end
    % Return the triangular form
    R = A;
end % function

end % methods

end % classdef


function sigVal = computeSigmoid(z)
% This function computes the value of the sigmoid of all of the numbers in
% an n x 1 vector, where n is the length of z.
%
% Input:
%   z: The n x 1 vector, where n is the length of z, of values whose
%   sigmoids need to be found
%
% Output:
%   sigVal: The value of the sigmoid

expz = exp(z);
sigVal = expz ./ (1 + expz);


function[u,v,w]=sphere2uvw(lat,lon,dr,dth,dphi)
%SPHERE2UVW Converts a 3D spherical vector to a 3D Cartesian vector.
%
%   [U,V,W]=XYZ2SPHERE(LAT,LON,V1,V2,V3) converts a vector in spherical
%   coordinates with components V1, V2, and V3 located at point
%   (LAT, LON) into a Cartesian 3-vector with components U, V, and W.
%
%   LAT and LON are in degrees.
%
%   The vector in the spherical coordinate system has radial component
%   V1, longitudinal component V2, and latitudinal component V3.
%
%   The Cartesian vector [U,V,W] is in a reference frame with the
%   X-axis at zero degrees longitude and the Z-axis at the North Pole.
%
%   All input arguments should be arrays of the same size.
%
%   SPHERE2UVW is inverted by UVW2SPHERE.
%
%   See JSPHERE for related functions.
%
%   Usage: [x,y,z]=sphere2uvw(lat,lon,v1,v2,v3);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2007--2014 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(lat, '--t')
    sphere2uvw_test,return
end
[phi,theta]=jdeg2rad(lat,lon);

u      =  cos(phi).*cos(theta).*dr  -sin(theta).*dth -  sin(phi).*cos(theta).*dphi;
v      =  cos(phi).*sin(theta).*dr + cos(theta).*dth -  sin(phi).*sin(theta).*dphi ;
w      =  sin(phi).*dr                                + cos(phi).*dphi;

function[]=sphere2uvw_test

lat=[0  0  45         0        -90]';
lon=[0  90 0         90         0 ]';
v1= [1  1  1          0         0]';
v2= [0  0  0         -1         0]';
v3= [0  0  0          0         1]';
u=  [1  0  sqrt(2)/2  1         1 ]';
v=  [0  1  0          0         0]';
w=  [0  0  sqrt(2)/2  0         0]';

[u2,v2,w2]=sphere2uvw(lat,lon,v1,v2,v3);
tol=1e-6;
reporttest('SPHERE2XYZ example points',aresame(u,u2,tol)&&aresame(v,v2,tol)&&aresame(w,w2,tol))


function	[out,dout]=rodrigues(in)

% RODRIGUES	Transform rotation matrix into rotation vector and viceversa.
%
%		Sintax:  [OUT]=RODRIGUES(IN)
% 		If IN is a 3x3 rotation matrix then OUT is the
%		corresponding 3x1 rotation vector
% 		if IN is a rotation 3-vector then OUT is the
%		corresponding 3x3 rotation matrix
%

%%
%%		Copyright (c) March 1993 -- Pietro Perona
%%		California Institute of Technology
%%

%% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995.
%% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !!

%% BUG when norm(om)=pi fixed -- April 6th, 1997;
%% Jean-Yves Bouguet

%% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003;
%% Jean-Yves Bouguet

[m,n] = size(in);
%bigeps = 10e+4*eps;
bigeps = 10e+20*eps;

if ((m==1) && (n==3)) | ((m==3) && (n==1)) %% it is a rotation vector
   theta = norm(in);
   if theta < eps
      R = eye(3);

      %if nargout > 1,

      dRdin = [0 0 0;
	       0 0 1;
	       0 -1 0;
	       0 0 -1;
	       0 0 0;
	       1 0 0;
	       0 1 0;
	       -1 0 0;
          0 0 0];

       %end;

   else
      if n==length(in)  in=in'; end; 	%% make it a column vec. if necess.

	 %m3 = [in,theta]

	 dm3din = [eye(3);in'/theta];

	 omega = in/theta;

	 %m2 = [omega;theta]

	 dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1];

	 alpha = cos(theta);
	 beta = sin(theta);
	 gamma = 1-cos(theta);
	 omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]];
	 A = omega*omega';

	 %m1 = [alpha;beta;gamma;omegav;A];

	 dm1dm2 = zeros(21,4);
	 dm1dm2(1,4) = -sin(theta);
	 dm1dm2(2,4) = cos(theta);
	 dm1dm2(3,4) = sin(theta);
	 dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0;
	                     0 0 -1 0 0 0 1 0 0;
			     0 1 0 -1 0 0 0 0 0]';

         w1 = omega(1);
	 w2 = omega(2);
	 w3 = omega(3);

	 dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0];
	 dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0];
	 dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3];

	 R = eye(3)*alpha + omegav*beta + A*gamma;

	 dRdm1 = zeros(9,21);

	 dRdm1([1 5 9],1) = ones(3,1);
	 dRdm1(:,2) = omegav(:);
	 dRdm1(:,4:12) = beta*eye(9);
	 dRdm1(:,3) = A(:);
	 dRdm1(:,13:21) = gamma*eye(9);

	 dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din;

      end;
      out = R;
      dout = dRdin;

      %% it is prob. a rot matr.
   elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)...
	    & (abs(det(in)-1) < bigeps))
      R = in;

      % project the rotation matrix to SO(3);
      [U,S,V] = svd(R);
      R = U*V';

      tr = (trace(R)-1)/2;
      dtrdR = [1 0 0 0 1 0 0 0 1]/2;
      theta = real(acos(tr));

      if sin(theta) >= 1e-5,

	 dthetadtr = -1/sqrt(1-tr^2);

	 dthetadR = dthetadtr * dtrdR;
	 % var1 = [vth;theta];
	 vth = 1/(2*sin(theta));
	 dvthdtheta = -vth*cos(theta)/sin(theta);
	 dvar1dtheta = [dvthdtheta;1];

	 dvar1dR =  dvar1dtheta * dthetadR;

	 om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]';

	 dom1dR = [0 0 0 0 0 1 0 -1 0;
	       0 0 -1 0 0 0 1 0 0;
	       0 1 0 -1 0 0 0 0 0];

	 % var = [om1;vth;theta];
	 dvardR = [dom1dR;dvar1dR];

	 % var2 = [om;theta];
	 om = vth*om1;
	 domdvar = [vth*eye(3) om1 zeros(3,1)];
	 dthetadvar = [0 0 0 0 1];
	 dvar2dvar = [domdvar;dthetadvar];

	 out = om*theta;
	 domegadvar2 = [theta*eye(3) om];

	 dout = domegadvar2 * dvar2dvar * dvardR;

      else
	 if tr > 0; 			% case norm(om)=0;

	    out = [0 0 0]';

	    dout = [0 0 0 0 0 1/2 0 -1/2 0;
		  0 0 -1/2 0 0 0 1/2 0 0;
		  0 1/2 0 -1/2 0 0 0 0 0];
	 else 				% case norm(om)=pi; %% fixed April 6th

	    out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]);
	    %keyboard;

	    if nargout > 1,
	       fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n');
		 	dout = NaN*ones(3,9);
	    end;
	 end;
      end;

   else
      error('Neither a rotation matrix nor a rotation vector were provided');
   end;

return;

%% test of the Jacobians:

%%%% TEST OF dRdom:
om = randn(3,1);
dom = randn(3,1)/1000000;

[R1,dR1] = rodrigues(om);
R2 = rodrigues(om+dom);

R2a = R1 + reshape(dR1 * dom,3,3);

gain = norm(R2 - R1)/norm(R2 - R2a)

%%% TEST OF dOmdR:
om = randn(3,1);
R = rodrigues(om);
dom = randn(3,1)/10000;
dR = rodrigues(om+dom) - R;

[omc,domdR] = rodrigues(R);
[om2] = rodrigues(R+dR);

om_app = omc + domdR*dR(:);

gain = norm(om2 - omc)/norm(om2 - om_app)

%%% OTHER BUG: (FIXED NOW!!!)

omu = randn(3,1);
omu = omu/norm(omu)
om = pi*omu;
[R,dR]= rodrigues(om);
[om2] = rodrigues(R);
[om om2]

%%% NORMAL OPERATION

om = randn(3,1);
[R,dR]= rodrigues(om);
[om2] = rodrigues(R);
[om om2]


function [ geom, iner, cpmo ] = polygeom( x, y )
%POLYGEOM Geometry of a planar polygon
%
%   POLYGEOM( X, Y ) returns area, X centroid,
%   Y centroid and perimeter for the planar polygon
%   specified by vertices in vectors X and Y.
%
%   [ GEOM, INER, CPMO ] = POLYGEOM( X, Y ) returns
%   area, centroid, perimeter and area moments of
%   inertia for the polygon.
%   GEOM = [ area   X_cen  Y_cen  perimeter ]
%   INER = [ Ixx    Iyy    Ixy    Iuu    Ivv    Iuv ]
%     u,v are centroidal axes parallel to x,y axes.
%   CPMO = [ I1     ang1   I2     ang2   J ]
%     I1,I2 are centroidal principal moments about axes
%         at angles ang1,ang2.
%     ang1 and ang2 are in radians.
%     J is centroidal polar moment.  J = I1 + I2 = Iuu + Ivv

% H.J. Sommer III - 02.05.14 - tested under MATLAB v5.2
%
% sample data
% x = [ 2.000  0.500  4.830  6.330 ]';
% y = [ 4.000  6.598  9.098  6.500 ]';
% 3x5 test rectangle with long axis at 30 degrees
% area=15, x_cen=3.415, y_cen=6.549, perimeter=16
% Ixx=659.561, Iyy=201.173, Ixy=344.117
% Iuu=16.249, Ivv=26.247, Iuv=8.660
% I1=11.249, ang1=30deg, I2=31.247, ang2=120deg, J=42.496
%
% H.J. Sommer III, Ph.D., Professor of Mechanical Engineering, 337 Leonhard Bldg
% The Pennsylvania State University, University Park, PA  16802
% (814)863-8997  FAX (814)865-9693  hjs1@psu.edu  www.me.psu.edu/sommer/

% begin function POLYGEOM

% check if inputs are same size
if ~isequal( size(x), size(y) ),
  error( 'X and Y must be the same size');
end

% number of vertices
[ x, ns ] = shiftdim( x );
[ y, ns ] = shiftdim( y );
[ n, c ] = size( x );

% temporarily shift data to mean of vertices for improved accuracy
xm = mean(x);
ym = mean(y);
x = x - xm*ones(n,1);
y = y - ym*ones(n,1);

% delta x and delta y
dx = x( [ 2:n 1 ] ) - x;
dy = y( [ 2:n 1 ] ) - y;

% summations for CW boundary integrals
A = sum( y.*dx - x.*dy )/2;
Axc = sum( 6*x.*y.*dx -3*x.*x.*dy +3*y.*dx.*dx +dx.*dx.*dy )/12;
Ayc = sum( 3*y.*y.*dx -6*x.*y.*dy -3*x.*dy.*dy -dx.*dy.*dy )/12;
Ixx = sum( 2*y.*y.*y.*dx -6*x.*y.*y.*dy -6*x.*y.*dy.*dy ...
          -2*x.*dy.*dy.*dy -2*y.*dx.*dy.*dy -dx.*dy.*dy.*dy )/12;
Iyy = sum( 6*x.*x.*y.*dx -2*x.*x.*x.*dy +6*x.*y.*dx.*dx ...
          +2*y.*dx.*dx.*dx +2*x.*dx.*dx.*dy +dx.*dx.*dx.*dy )/12;
Ixy = sum( 6*x.*y.*y.*dx -6*x.*x.*y.*dy +3*y.*y.*dx.*dx ...
          -3*x.*x.*dy.*dy +2*y.*dx.*dx.*dy -2*x.*dx.*dy.*dy )/24;
P = sum( sqrt( dx.*dx +dy.*dy ) );

% check for CCW versus CW boundary
if A < 0,
  A = -A;
  Axc = -Axc;
  Ayc = -Ayc;
  Ixx = -Ixx;
  Iyy = -Iyy;
  Ixy = -Ixy;
end

% centroidal moments
xc = Axc / A;
yc = Ayc / A;
Iuu = Ixx - A*yc*yc;
Ivv = Iyy - A*xc*xc;
Iuv = Ixy - A*xc*yc;
J = Iuu + Ivv;

% replace mean of vertices
x_cen = xc + xm;
y_cen = yc + ym;
Ixx = Iuu + A*y_cen*y_cen;
Iyy = Ivv + A*x_cen*x_cen;
Ixy = Iuv + A*x_cen*y_cen;

% principal moments and orientation
I = [ Iuu  -Iuv ;
     -Iuv   Ivv ];
[ eig_vec, eig_val ] = eig(I);
I1 = eig_val(1,1);
I2 = eig_val(2,2);
ang1 = atan2( eig_vec(2,1), eig_vec(1,1) );
ang2 = atan2( eig_vec(2,2), eig_vec(1,2) );

% return values
geom = [ A  x_cen  y_cen  P ];
iner = [ Ixx  Iyy  Ixy  Iuu  Ivv  Iuv ];
cpmo = [ I1  ang1  I2  ang2  J ];

% end of function POLYGEOM


function value = r8_log_2 ( x )

%*****************************************************************************80
%
%% R8_LOG_2 returns the logarithm base 2 of |X|.
%
%  Discussion:
%
%    R8_LOG_2 ( X ) = Log ( |X| ) / Log ( 2.0 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the number whose base 2 logarithm is desired.
%    X should not be 0.
%
%    Output, real VALUE, the logarithm base 2 of the absolute
%    value of X.  It should be true that |X| = 2**R8_LOG_2.
%
  if ( x == 0.0 )
    value = -inf;
  else
    value = log ( abs ( x ) ) / log ( 2.0 );
  end

  return
end


function centroids = computeCentroids(X, idx, K)
%COMPUTECENTROIDS returs the new centroids by computing the means of the
%data points assigned to each centroid.
%   centroids = COMPUTECENTROIDS(X, idx, K) returns the new centroids by
%   computing the means of the data points assigned to each centroid. It is
%   given a dataset X where each row is a single data point, a vector
%   idx of centroid assignments (i.e. each entry in range [1..K]) for each
%   example, and K, the number of centroids. You should return a matrix
%   centroids, where each row of centroids is the mean of the data points
%   assigned to it.
%

% Useful variables
[m n] = size(X);

% You need to return the following variables correctly.
centroids = zeros(K, n);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every centroid and compute mean of all points that
%               belong to it. Concretely, the row vector centroids(i, :)
%               should contain the mean of the data points assigned to
%               centroid i.
%
% Note: You can use a for-loop over the centroids to compute this.
%

for i = 1:K
    idx_i = idx == i;
    centroids(i, :) = (idx_i' * X) ./ sum(idx_i);
end

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

end


function [kEst,thetaEst]=PoissonRateLikeGammaConjUpdate(xMeas,kEst,thetaEst)
%%POISSONRATELIKEGAMMACONJUPDATE When estimating the rate parameter of the
%               Poisson distribution, the conjugate prior distribution is a
%               central (lambda=0) gamma distribution. Given the prior
%               shape and rate parameters of the gamma distribution, this
%               function updates those parameters conditioned on the
%               measurements. The result is the parameters of the posterior
%               distribution, which is also a gamma distribution.
%
%INPUTS: xMeas An NX1 or 1XN set of N independent measurements of the
%              Poisson distribution.
%         kEst The shape parameter of the prior (central gamma)
%              distribution.
%     thetaEst The scale parameter of the prior distribution.
%
%OUTPUTS: kEst The shape parameter of the posterior (central gamma)
%              distribution.
%     thetaEst The scale parameter of the posterior distribution.
%
%A distribution that is conjugate prior to a particular likelihood function
%is such that after performing Bayes' rule to update using a measurement,
%one obtains the same type of distribution back again. This function just
%performs Bayes rule with the given measurements.
%
%The conjugate prior relation for the Poisson distribution is given in [1].
%
%REFERENCES:
%[1] D. Fink, "A compendium of conjugate priors," Montana State University,
%    Department of Biology, Environmental Statistics Group, Tech. Rep., May
%    1997. [Online].
%    Available: http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.157.5540
%
%October 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numMeas=length(xMeas);

kEst=kEst+sum(xMeas);
if(isfinite(thetaEst))
    thetaEst=thetaEst/(numMeas*thetaEst+1);
else%If an uninformativ prior is given (thetaEst=Inf), use the asymptotic
    %result.
    thetaEst=1/numMeas;
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 value = sphere_shell_volume_nd ( n, r1, r2 )

%*****************************************************************************80
%
%% SPHERE_SHELL_VOLUME_ND computes the volume of a spherical shell in ND.
%
%  Integration region:
%
%    Points X(1:N) such that:
%
%      R1**2 <= Sum ( X(1:N) - XC(1:N) )**2 <= R2**2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the space.
%
%    Input, real R1, R2, the radiuses of the inner and
%    outer spheres.
%
%    Output, real SPHERE_SHELL_VOLUME_ND, the volume of the
%    spherical shell.
%
  value = ball_volume_nd ( n, r2 ) - ball_volume_nd ( n, r1 );

  return
end


function rx = vecrotx(angle)
%
% Function Name:
%
%   vecrotx - Transformation matrix for a rotation around the x axis.
%
% Calling Sequence:
%
%   rx = vecrotx(angle);
%
% Parameters:
%
%   angle		: rotation angle defined in radians
%
%   rx		: (4x4) Transformation matrix.
%
%
% Description:
%
%   Return the (4x4) Transformation matrix for a rotation about the x axis
%   by the defined angle.
%
%   The matrix is:
%
%         [ 1         0            0          0]
%         [ 0     cos(angle)  -sin(angle)     0]
%         [ 0     sin(angle)   cos(angle)     0]
%         [ 0         0            0          1]
%
% Examples:
%
%    Rotate the NURBS line (0.0 0.0 0.0) - (3.0 3.0 3.0) by 45 degrees
%    around the x-axis
%
%    line = nrbline([0.0 0.0 0.0],[3.0 3.0 3.0]);
%    trans = vecrotx(%pi/4);
%    rline = nrbtform(line, trans);
%
% See:
%
%    nrbtform

%  Dr D.M. Spink
%  Copyright (c) 2000.

sn = sin(angle);
cn = cos(angle);
rx = [1 0 0 0; 0 cn -sn 0; 0 sn cn 0; 0 0 0 1];


function circle_forward ( a )

%*****************************************************************************80
%
%% CIRCLE_FORWARD plots the image of the unit circle under the 2x2 matrix A.
%
%  Discussion:
%
%    A typical 2x2 matrix will map the unit circle to some kind of tilted ellipse.
%    The aspect ratio of the ellipse (ratio of major to minor axes) is a measure
%    of the conditioning of the matrix.
%
%    A singular matrix maps the unit circle to a line.
%
%    A diagonal matrix maps the unit circle to an ellipse with no tilting.
%    (There is no rotation.)
%
%    An orthogonal matrix maps the unit circle to the unit circle (but points
%    may have rotated.)
%
%    The identity matrix maps the unit circle to itself.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A(2,2), the matrix whose mapping of the unit circle
%    is to be studied.
%

%
%  This call produces matrices U, S and V.
%
% [ u, s, v ] = svd ( a );
%
%  This call produces just the singular values, and in a vector, not a matrix.
%
  s = svd ( a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Singular values of A are %f and %f\n', s(1), s(2) );
%
%  Select N+1 evenly spaced points X on the unit circle.
%
  n = 32;

  i = [ 0 : n ];

  angle = 2 * pi * i / n;

  r = 1;

  x = [ r * cos(angle); r * sin(angle) ];
%
%  AX contains the image of each point under the mapping X -> A*X.
%
  ax =  a * x;
%
%  Plot the corners of the region.  This is a trick to force MATLAB
%  to plot in a square with equal axes.  I don't know why it seems to
%  hard to get this to happen in general!
%
  s = [];

  p_min = min ( min ( min ( ax ) ), min ( min ( x ) ) );
  p_max = max ( max ( max ( ax ) ), max ( max ( x ) ) );

  scatter ( [ p_min, p_max, p_max, p_min ], [ p_min, p_min, p_max, p_max ], s, 'k' );
  axis ( [ p_min, p_max, p_min, p_max ] );

  axis equal
  axis tight
  axis square

  hold on
%
%  Plot the points X on the unit circle.
%
  scatter ( x(1,:), x(2,:), s, 'b', 'filled' );
%
%  Plot the points AX on the image of the unit circle.
%
  scatter ( ax(1,:), ax(2,:), s, 'r', 'filled' );
%
%  Plot vectors for "3-oclock" and "12-oclock".
%
  line ( [ 0, x(1,1)], [ 0, x(2,1)], 'color', 'b' );
  line ( [ 0, x(1,n/4+1)], [ 0, x(2,n/4+1)], 'color', 'b' );
%
%  Plot the images of "3-oclock" and "12-oclock".
%
  line ( [ 0, ax(1,1)], [ 0, ax(2,1)], 'color', 'r' );
  line ( [ 0, ax(1,n/4+1)], [ 0, ax(2,n/4+1)], 'color', 'r' );

  xlabel ( '--X axis--' )
  ylabel ( '--Y axis--' )
  title ( 'X in blue, A*X in red' );

  hold off

  return
end


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

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

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

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

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

for i=1:num_movies
    idx = find(R(i, :) == 1);
    Theta_temp = Theta(idx, :);
    Y_temp = Y(i, idx);
    X_grad(i, :) = (X(i, :) * Theta_temp' - Y_temp) * Theta_temp + lambda * X(i, :);
end

for i=1:num_users
    idx = find(R(:, i) == 1);
    X_temp = X(idx, :);
    Y_temp = Y(idx, i);
    Theta_grad(i, :) = (X_temp * Theta(i, :)' - Y_temp)' * X_temp + lambda * Theta(i, :);
end

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

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

end


function r=rot(psi,theta,phi);
% function r=rot(psi,theta,phi)
%
% Description
%  Computes the rotarion matrix corresponding to a oritentation vector
% The input data are:
% - psi   .- the psi angle (radians)
% - theta .- the theta angle (radians)
% - phi   .- the phi angle (radians)
%
% The return value is
% the rotation matrix (dim 3x3)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%               Grupo de Robotica                                          %%
%%               Departamento de Informatica e Ingenieria de Sistemas       %%
%%               Universidad de Zaragoza                                    %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Name         : .m                                              %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Programmer   : Jose Maria Martinez                                       %%
%% Languaje     : Matlab 4.2 c                                              %%
%% Date         : February 1996                                             %%
%% Status       : Prueba                                                    %%
%% History	: 16-8-95 creation                                          %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

cosphi=cos(phi);
sinphi=sin(phi);
costheta=cos(theta);
sintheta=sin(theta);
cospsi=cos(psi);
sinpsi=sin(psi);

r=zeros(3,3);
r(1,1)=cosphi*costheta;
r(1,2)=cosphi*sintheta*sinpsi-sinphi*cospsi;
r(1,3)=cosphi*sintheta*cospsi+sinphi*sinpsi;
r(2,1)=sinphi*costheta;
r(2,2)=sinphi*sintheta*sinpsi+cosphi*cospsi;
r(2,3)=sinphi*sintheta*cospsi-cosphi*sinpsi;
r(3,1)=-sintheta;
r(3,2)=costheta*sinpsi;
r(3,3)=costheta*cospsi;
return;


function call_price=american_call_baw(S, X, r, b, sigma, time, accuracy)

%--------------------------------------------------------------------------
%
% DESCRIPTION:
%
% Barone-Adesi and Whaley (1987) quadratic approximation to the price
% of a call option.
%
%
% Reference:
%
% Giovanni Barone-Adesi and Robert E. Whaley,
% "Efficient analytic approximation of American option values",
% Journal of Finance, 42(2):301-320, June 1987.
%
%
%--------------------------------------------------------------------------
%
% INPUTS:
%
%   S:          spot price
%   X:          exercise price
%   r:          interest rate
%   b:          dividend yield
%   sigma:      volatility
%   time:       time to maturity
%   accuracy:   approximation accuracy
%
%--------------------------------------------------------------------------
%
% OUTPUT:
%
% call_price: price of a call option
%
%--------------------------------------------------------------------------
%
% Author:  Paolo Z., February 2012
%
%--------------------------------------------------------------------------

sigma_sqr = sigma*sigma;
time_sqrt = sqrt(time);

nn = 2.0*b/sigma_sqr;

m = 2.0*r/sigma_sqr;
K = 1.0-exp(-r*time);

q2 = ( -(nn-1) + sqrt( (nn-1)^2 + (4*m/K) ) )*0.5;
q2_inf = 0.5 * ( -(nn-1) + sqrt( (nn-1)^2 + 4.0*m ) );

S_star_inf = X / (1.0 - 1.0/q2_inf);
h2 = -(b*time+2.0*sigma*time_sqrt)*(X/(S_star_inf-X));

S_seed = X + (S_star_inf-X)*(1.0-exp(h2));

no_iterations=0;

Si     = S_seed;
g      = 1;
gprime = 1;

while ((abs(g) > accuracy) && (abs(gprime)>accuracy) && ( no_iterations<500) && (Si>0.0))
    c = european_call_contpay(Si,X,r,b,sigma,time);

    d1     = (log(Si/X)+(b+0.5*sigma_sqr)*time)/(sigma*time_sqrt);
    g      =(1.0-1.0/q2)*Si-X-c+(1.0/q2)*Si*exp((b-r)*time)*normcdf(d1);

    gprime =( 1.0-1.0/q2)*(1.0-exp((b-r)*time)*normcdf(d1))+(1.0/q2)*exp((b-r)*time)*normcdf(d1)*(1.0/(sigma*time_sqrt));
    Si     = Si-(g/gprime);
end

S_star = 0;

if (abs(g)>accuracy)
    S_star = S_seed; %// did not converge
else
    S_star = Si;
end

C=0;
c = european_call_contpay(S,X,r,b,sigma,time);

if (S>=S_star)
    C  = S-X;
else
    d1 = (log(S_star/X)+(b+0.5*sigma_sqr)*time)/(sigma*time_sqrt);
    A2 = (1.0-exp((b-r)*time)*normcdf(d1))* (S_star/q2);
    C  = c+A2*((S/S_star)^q2);
end

call_price = max(C,c);


function [ alpha, beta, gamma ] = bdf_set ( n )

%*****************************************************************************80
%
%% BDF_SET sets weights for backward differentiation ODE weights.
%
%  Discussion:
%
%    GAMMA * Y(N+1) = Sum ( 1 <= I <= N ) ALPHA(I) * Y(N+1-I)
%                     + dX * BETA * Y'(X(N+1),Y(N+1))
%
%    This is equivalent to the backward differentiation corrector formulas.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order.
%    N must be between 1 and 6.
%
%    Output, real ALPHA(N), BETA, GAMMA, the weights.
%
  alpha = zeros ( n, 1 );

  if ( n == 1 )
    beta =     1.0;
    gamma =    1.0;
    alpha(1) = 1.0;
  elseif ( n == 2 )
    beta =       2.0;
    gamma =      3.0;
    alpha(1) =   4.0;
    alpha(2) = - 1.0;
  elseif ( n == 3 )
    beta =       6.0;
    gamma =     11.0;
    alpha(1) =  18.0;
    alpha(2) = - 9.0;
    alpha(3) =   2.0;
  elseif ( n == 4 )
    beta =       12.0;
    gamma =      25.0;
    alpha(1) =   48.0;
    alpha(2) = - 36.0;
    alpha(3) =   16.0;
    alpha(4) =  - 3.0;
  elseif ( n == 5 )
    beta =        60.0;
    gamma =      137.0;
    alpha(1) =   300.0;
    alpha(2) = - 300.0;
    alpha(3) =   200.0;
    alpha(4) =  - 75.0;
    alpha(5) =    12.0;
  elseif ( n == 6 )
    beta =        60.0;
    gamma =      147.0;
    alpha(1) =   360.0;
    alpha(2) = - 450.0;
    alpha(3) =   400.0;
    alpha(4) = - 225.0;
    alpha(5) =    72.0;
    alpha(6) =  - 10.0;
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BDF_SET - Fatal error!\n' );
    fprintf ( 1, '  Illegal order requested = %d\n', n );
    error ( 'BDF_SET - Fatal error!' );
  end

  return
end