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


function mean = truncated_normal_ab_mean ( mu, sigma, a, b )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_AB_MEAN returns the mean of the truncated Normal PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real MU, SIGMA, the mean and standard deviation of the
%    parent Normal distribution.
%
%    Input, real A, B, the lower and upper truncation limits.
%
%    Output, real MEAN, the mean of the PDF.
%
  alpha = ( a - mu ) / sigma;
  beta = ( b - mu ) / sigma;

  alpha_cdf = normal_01_cdf ( alpha );
  beta_cdf = normal_01_cdf ( beta );

  alpha_pdf = normal_01_pdf ( alpha );
  beta_pdf = normal_01_pdf ( beta );

  mean = mu + sigma * ( alpha_pdf - beta_pdf ) / ( beta_cdf - alpha_cdf );

  return
end


function value = r8vec_distance ( dim_num, v1, v2 )

%*****************************************************************************80
%
%% R8VEC_DISTANCE returns the Euclidean distance between two R8VEC's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real V1(DIM_NUM), V2(DIM_NUM), the vectors.
%
%    Output, real VALUE, the Euclidean distance
%    between the vectors.
%
  value = sqrt ( sum ( ( v1(1:dim_num) - v2(1:dim_num) ).^2 ) );

  return
end


function [U, S] = pca(X)
%PCA Run principal component analysis on the dataset X
%   [U, S, X] = pca(X) computes eigenvectors of the covariance matrix of X
%   Returns the eigenvectors U, the eigenvalues (on diagonal) in S
%

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

% You need to return the following variables correctly.
U = zeros(n);
S = zeros(n);

% ====================== YOUR CODE HERE ======================
% Instructions: You should first compute the covariance matrix. Then, you
%               should use the "svd" function to compute the eigenvectors
%               and eigenvalues of the covariance matrix.
%
% Note: When computing the covariance matrix, remember to divide by m (the
%       number of examples).
%
Sigma=(1./m).*X'*X;
[U, S, V]=svd(Sigma);

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

end


function [Q, R] = gs_m(A)
%GS_M    Modified Gram-Schmidt QR factorization.
%        [Q, R] = GS_M(A) uses the modified Gram-Schmidt method to compute the
%        factorization A = Q*R for m-by-n A of full rank,
%        where Q is m-by-n with orthonormal columns and R is n-by-n.

%        Reference:
%        N. J. Higham, Accuracy and Stability of Numerical Algorithms,
%        Second edition, Society for Industrial and Applied Mathematics,
%        Philadelphia, PA, 2002; sec 19.8.

[m, n] = size(A);
Q = zeros(m,n);
R = zeros(n);

for k=1:n
    R(k,k) = norm(A(:,k));
    Q(:,k) = A(:,k)/R(k,k);
    R(k,k+1:n) = Q(:,k)'*A(:,k+1:n);
    A(:,k+1:n) = A(:,k+1:n) - Q(:,k)*R(k,k+1:n);
end


function cdf = gompertz_cdf ( x, a, b )

%*****************************************************************************80
%
%% GOMPERTZ_CDF evaluates the Gompertz CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Johnson, Kotz, and Balakrishnan,
%    Continuous Univariate Distributions, Volume 2, second edition,
%    Wiley, 1994, pages 25-26.
%
%  Parameters:
%
%    Input, real X, the argument of the CDF.
%
%    Input, real A, B, the parameters of the PDF.
%    1 < A, 0 < B.
%
%    Output, real CDF, the value of the CDF.
%
  if ( x <= 0.0 )
    cdf = 0.0;
  else
    cdf = 1.0 - exp ( - b * ( a^x - 1.0 ) / log ( a ) );
  end

  return
end


function v = haar_2d ( u )

%*****************************************************************************80
%
%% HAAR_2D computes the Haar transform of an array.
%
%  Discussion:
%
%    Thanks to Stephen Becker for pointing out that a previous version of
%    the haar_2d code was not inverted by haar_2d_inverse in cases where
%    M and N were not powers of 2, 05 March 2014.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real U(M,N), the vector to be transformed.
%
%    Output, real V(M,N), the transformed vector.
%
  [ m, n ] = size ( u );

  v = u;

  s = sqrt ( 2.0 );

  w = zeros ( m, n );
%
%  Determine K, the largest power of 2 such that K <= M.
%
  k = 1;
  while ( k * 2 <= m )
    k = k * 2;
  end
%
%  Transform all columns.
%
  while ( 1 < k )

    k = floor ( k / 2 );

    w(  1:  k,:) = ( v(1:2:2*k-1,:) + v(2:2:2*k,:) ) / s;
    w(k+1:k+k,:) = ( v(1:2:2*k-1,:) - v(2:2:2*k,:) ) / s;

    v(1:2*k,:) = w(1:2*k,:);

  end
%
%  Determine K, the largest power of 2 such that K <= N.
%
  k = 1;
  while ( k * 2 <= n )
    k = k * 2;
  end
%
%  Transform all rows.
%
  while ( 1 < k )

    k = floor ( k / 2 );

    w(:,  1:  k) = ( v(:,1:2:2*k-1) + v(:,2:2:2*k) ) / s;
    w(:,k+1:k+k) = ( v(:,1:2:2*k-1) - v(:,2:2:2*k) ) / s;

    v(:,1:2*k) = w(:,1:2*k);

  end

  return
end


function value = ellipse_area_2d ( r1, r2 )

%*****************************************************************************80
%
%% ELLIPSE_AREA_2D returns the area of an ellipse in 2D.
%
%  Discussion:
%
%    The ellipse is defined as points (X,Y) such that
%
%      ( ( X - XC ) / R1 )^2 + ( ( Y - YC ) / R2 )^2 <= 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R1, R2, the major and minor semi-axes.
%
%    Output, real ELLIPSE_AREA_2D, the area of the ellipse.
%
  value = pi * r1 * r2;

  return
end


function pdf = gamma_pdf ( x, a, b, c )

%*****************************************************************************80
%
%% GAMMA_PDF evaluates the Gamma PDF.
%
%  Discussion:
%
%    PDF(X)(A,B,C) = EXP ( - ( X - A ) / B ) * ( ( X - A ) / B )^(C-1)
%      / ( B * GAMMA ( C ) )
%
%    GAMMA_PDF(A,B,C), where C is an integer, is the Erlang PDF.
%    GAMMA_PDF(A,B,1) is the Exponential PDF.
%    GAMMA_PDF(0,2,C/2) is the Chi Squared PDF with C degrees of freedom.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%    A <= X.
%
%    Input, real A, B, C, the parameters of the PDF.
%    A controls the location of the peak;  A is often chosen to be 0.0.
%    B is the "scale" parameter; 0.0 < B, and is often 1.0.
%    C is the "shape" parameter; 0.0 < C, and is often 1.0.
%
%    Output, real PDF, the value of the PDF.
%
  if ( x <= a )

    pdf = 0.0;

  else

    y = ( x - a ) / b;

    pdf = y^( c - 1.0 ) / ( b * gamma ( c ) * exp ( y ) );

  end

  return
end


function xyz = sphere_cubed_ijk_to_xyz ( n, i, j, k )

%*****************************************************************************80
%
%% SPHERE_CUBED_IJK_TO_XYZ: cubed sphere IJK to XYZ coordinates.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sections into which each face of
%    the cube is to be divided.
%
%    Input, integer I, J, K, indices between 0 and N.  Normally,
%    at least one of the indices should have the value 0 or N.
%
%    Output, real XYZ(3,1), coordinates of the point.
%
  if ( i == 0 )
    xc = -1.0;
  elseif ( i == n )
    xc = +1.0;
  else
    xc = tan ( ( 2 * i - n ) * 0.25 * pi / n );
  end

  if ( j == 0 )
    yc = -1.0;
  elseif ( j == n )
    yc = +1.0;
  else
    yc = tan ( ( 2 * j - n ) * 0.25 * pi / n );
  end

  if ( k == 0 )
    zc = -1.0;
  elseif ( k == n )
    zc = +1.0;
  else
    zc = tan ( ( 2 * k - n ) * 0.25 * pi / n );
  end

  xyzn = sqrt ( xc^2 + yc^2 + zc^2 );

  xyz = zeros ( 3, 1 );

  xyz(1) = xc / xyzn;
  xyz(2) = yc / xyzn;
  xyz(3) = zc / xyzn;

  return
end


function variance = r8row_variance ( m, n, x )

%*****************************************************************************80
%
%% R8ROW_VARIANCE returns the variances of an R8ROW.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in the array.
%
%    Input, real X(M,N), the R8ROW whose row means are desired.
%
%    Output, real VARIANCE(M), the variances of the rows of X.
%
  for i = 1 : m

    mean = sum ( x(i,1:n) ) / n;

    variance(i) = sum ( ( x(i,1:n) - mean ).^2 );

    if ( 1 < n )
      variance(i) = variance(i) / ( n - 1 );
    else
      variance(i) = 0.0;
    end

  end

  return
end


function [Qx,Qy]=CubicBezier1(Px,Py,n)
% Input:
% Px contains x-coordinates of control points [Px0,Px1,Px2,Px3]
% Py contains y-coordinates of control points [Py0,Py1,Py2,Py3]
% n is number of intervals (uniform parameterization)

%Output:
% Qx contains parameteric evaluted x-value
% Qy contains parameteric evaluted y-value

% Equation of Bezier Curve, utilizes Horner's rule for efficient computation.
% Q(t)=(-P0 + 3*(P1-P2) + P3)*t^3 + 3*(P0-2*P1+P2)*t^2 + 3*(P1-P0)*t + Px0

Px0=Px(1);
Py0=Py(1);
Px1=Px(2);
Py1=Py(2);
Px2=Px(3);
Py2=Py(3);
Px3=Px(4);
Py3=Py(4);

cx3=-Px0 + 3*(Px1-Px2) + Px3;
cy3=-Py0 + 3*(Py1-Py2) + Py3;
cx2=3*(Px0-2*Px1+Px2);
cy2=3*(Py0-2*Py1+Py2);
cx1=3*(Px1-Px0);
cy1=3*(Py1-Py0);
cx0=Px0;
cy0=Py0;

dt=1/n;
Qx(1)=Px0; % Qx at t=0
Qy(1)=Py0; % Qy at t=0
for i=1:n
    t=i*dt;
    Qx(i+1)=((cx3*t+cx2)*t+cx1)*t + cx0;
    Qy(i+1)=((cy3*t+cy2)*t+cy1)*t + cy0;
end

% % % --------------------------------
% % % Author: Dr. Murtaza Khan
% % % Email : drkhanmurtaza@gmail.com
% % % --------------------------------


% Minimum volume ellipsoid covering a finite set
% Section 8.4.1, Boyd & Vandenberghe "Convex Optimization"
% Almir Mutapcic - 10/05
% (a figure is generated)
%
% Given a finite set of points x_i in R^2, we find the minimum volume
% ellipsoid (described by matrix A and vector b) that covers all of
% the points by solving the optimization problem:
%
%           maximize     log det A
%           subject to   || A x_i + b || <= 1   for all i
%
% CVX cannot yet handle the logdet function, but this problem can be
% represented in an equivalent way as follows:
%
%           maximize     det(A)^(1/n)
%           subject to   || A x_i + b || <= 1   for all i
%
% The expression det(A)^(1/n) is SDP-representable, and is implemented
% by the MATLAB function det_rootn().

% Generate data
x = [ 0.55  0.0;
      0.25  0.35
     -0.2   0.2
     -0.25 -0.1
     -0.0  -0.3
      0.4  -0.2 ]';
[n,m] = size(x);

% Create and solve the model
cvx_begin
    variable A(n,n) symmetric
    variable b(n)
    maximize( det_rootn( A ) )
    subject to
        norms( A * x + b * ones( 1, m ), 2 ) <= 1;
cvx_end

% Plot the results
clf
noangles = 200;
angles   = linspace( 0, 2 * pi, noangles );
ellipse  = A \ [ cos(angles) - b(1) ; sin(angles) - b(2) ];
plot( x(1,:), x(2,:), 'ro', ellipse(1,:), ellipse(2,:), 'b-' );
axis off


function area = polygonArea(poly, varargin)
%POLYGONAREA Compute the signed area of a polygon
%
%   A = polygonArea(POINTS);
%   Compute area of a polygon defined by POINTS. POINTS is a N-by-2 array
%   of double containing coordinates of vertices.
%
%   Vertices of the polygon are supposed to be oriented Counter-Clockwise
%   (CCW). In this case, the signed area is positive.
%   If vertices are oriented Clockwise (CW), the signed area is negative.
%
%   If polygon is self-crossing, the result is undefined.
%
%   Examples
%     % compute area of a simple shape
%     poly = [10 10;30 10;30 20;10 20];
%     area = polygonArea(poly)
%     area =
%         200
%
%     % compute area of CW polygon
%     area2 = polygonArea(poly(end:-1:1, :))
%     area2 =
%         -200
%
%     % Computes area of a paper hen
%     x = [0 10 20  0 -10 -20 -10 -10  0];
%     y = [0  0 10 10  20  10  10  0 -10];
%     poly = [x' y'];
%     area = polygonArea(poly)
%     area =
%        400
%
%     % Area of unit square with 25% hole
%     pccw = [0 0; 1 0; 1 1; 0 1];
%     pcw = pccw([1 4 3 2], :) * .5 + .25;
%     polygonArea ([pccw; nan(1,2); pcw])
%     ans =
%        0.75
%
%   References
%   algo adapted from P. Bourke web page
%   http://paulbourke.net/geometry/polygonmesh/
%
%   See also:
%   polygons2d, polygonCentroid, polygonSecondAreaMoments, triangleArea
%

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

%   HISTORY
%   25/04/2005: add support for multiple polygons
%   12/10/2007: update doc

%% Process special cases

% in case of polygon sets, computes the sum of polygon areas
if iscell(poly)
    area = 0;
    for i = 1:length(poly)
        area = area + polygonArea(poly{i});
    end
    return;
end

% check there are enough points
if size(poly, 1) < 2
    area = 0;
    return;
end

% case of polygons with holes -> computes the sum of areas
if any(isnan(poly))
    area = sum(polygonArea(splitPolygons(poly)));
    return;
end

%% Process single polygons or single rings

% extract coordinates
if nargin == 1
    % polygon given as N-by-2 array
    px = poly(:, 1);
    py = poly(:, 2);

elseif nargin == 2
    % poylgon given as two N-by-1 arrays
    px = poly;
    py = varargin{1};
end

% indices of next vertices
N = length(px);
iNext = [2:N 1];

% compute area (vectorized version)
area = sum(px .* py(iNext) - px(iNext) .* py) / 2;


function f=shah(L,a);
%SHAH  Discrete Shah-distribution
%   Usage: f=shah(L,a);
%
%   `shah(L,a)` computes the discrete, normalized Shah-distribution of
%   length *L* with a distance of *a* between the spikes.
%
%   The Shah distribution is defined by
%
%   .. f(n*a+1)=1/sqrt(L/a)
%
%   .. math:: f(n\cdot a+1)=\frac{1}{\sqrt(L/a)}
%
%   for integer *n*, otherwise *f* is zero.
%
%   This is also known as an impulse train or as the comb function, because
%   the shape of the function resembles a comb. It is the sum of unit
%   impulses ('diracs') with the distance *a*.
%
%   If *a* divides *L*, then the |dft| of `shah(L,a)` is `shah(L,L/a)`.
%
%   The Shah function has an extremely bad time-frequency localization.
%   It does not generate a Gabor frame for any *L* and *a*.
%
%   Examples:
%   ---------
%
%   A simple spectrogram of the Shah function (includes the negative
%   frequencies to display the whole TF-plane):::
%
%     sgram(shah(256,16),'dynrange',80,'nf')
%

%   AUTHOR : Peter L. Søndergaard
%   TESTING: OK
%   REFERENCE: OK

if nargin~=2
  error('Wrong number of input parameters.');
end;

%if mod(L,a)~=0
%  error('a must divide L.');
%end;

f=zeros(L,1);

f(1:a:L)=1/sqrt(L/a);


function mi = imMutualInformation(img1, img2)
%IMMUTUALINFORMATION  Mutual information between two images
%
%   MI = imMutualInformation(IMG1, IMG2)
%   Computes the mutual information between two images.
%   The mutual information can be related to entropy and joint entropy:
%     MI = H1 + H2 - H12
%   where H1 and H2 are the entropies computed on images IMG1 and IMG2, and
%   H12 is the joint entropy of images IMG1 and IMG2.
%
%   The mutual information between two independant images corresponds to
%   the sum of the entropies computed for each image. The mutual
%   information of an image with itself equals the entropy of this image.
%
%   Example
%   % compute mutual information between an image and a shifted copy of
%   % itself
%     img = imread('cameraman.tif');
%     img2 = circshift(img, [2 3]);
%     MI = imMutualInformation(img, img2)
%     img3 = circshift(img, [5, 7]);
%     MI3 = imMutualInformation(img, img2)
%
%   % Check that we get same results by computing entropies (could be wrong
%   % when computed on double images)
%     img1 = imread('rice.png');
%     img2 = circshift(img1, [3, 4]);
%     imMutualInformation(img1, img2)
%     ans =
%         1.0551
%     h1 = imEntropy(img1);
%     h2 = imEntropy(img2);
%     h12 = imJointEntropy(img1, img2);
%     h1 + h2 - h12
%     ans =
%         1.0551

%   See also
%   imJointEntropy, imJointHistogram, imEntropy
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-08-26,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% joint histogram, and reduced hisograms
hist12  = imJointHistogram(img1, img2);
hist1   = sum(hist12, 2);
hist2   = sum(hist12, 1);

% normalisation of histograms
hist12  = hist12(hist12 > 0) / sum(hist12(:));
hist1   = hist1(hist1 > 0) / sum(hist1(:));
hist2   = hist2(hist2 > 0) / sum(hist2(:));

% entropy of each image
h12 = -sum(hist12 .* log2(hist12));
h1  = -sum(hist1  .* log2(hist1));
h2  = -sum(hist2  .* log2(hist2));

% compute mutual information
mi = h1 + h2 - h12;


function [xn, xmean, xstd] = normdata(x,xmean,xstd)
%NORMDATA  Normalize input to zero mean and unit variance
%
%  Description
%    [XN, XMEAN, XSTD] = NORMDATA(X) normalizes X to zero mean and
%    unit variance. Returns normalized XN, mean of X XMEAN and
%    standard deviance of X XSTD.
%
%    XN = NORMDATA(X,XMEAN,XSTD) normalizes X using precomputed
%    XMEAN and XSTD.
%
%  See also DENORMDATA
%
% Copyright (c) 2010 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  if nargin<=1
    xmean=nanmean(x);
    xstd=nanstd(x);
  end
  xn=bsxfun(@rdivide,bsxfun(@minus,x,xmean),xstd);