function [XNorm mu stddev] = normalizeFeatures(X)

% This function is used to normalize the features. It makes the mean of
% each feature 0. Formula for normalizing feature taking all examples into
% consideration is : [X(particular feature) - mean(of feature)]/ standard
% deviation of feature.

mu = mean(X);
stddev = std(X);

XNorm = bsxfun(@minus,X,mu); % subtract mean of each feature from original value
XNorm = bsxfun(@rdivide,XNorm,stddev); % divide by standard deviation


%
%   Calculate the scalar multiplication of a scalar t and vector u
%
%  Syntax:  >> v = VectorScale(s,u)
%
%   Input:  t --- (numeric) the scalar
%           u --- (matrix/string/cell) a general vector that is either
%                (i) an mxn matrix, or
%                (ii) a polynomial in character string, or
%                (iii) a 1xk cell array, each u{j} is either (i) or (ii)
%
%  Example:
% >> v = VectorScale(3,{[1 2 3; 4 5 6], '7+8*x*y+9*z^3'});
% >> v{:}
% ans =
%      3     6     9
%     12    15    18
% ans =
% 21 + 24*x*y + 27*z^3
%


%
% Performs contour (circular) integral with
% center x_0,y_0 and radius r, using discrete riemann approach.
% The angs parameter specifies region of the circle
% considering clockwise 0-2pi notation.
% Result not normalized.
% Author: J. Rouces
%
function sum = ContourIntegralCircular(imagen,y_0,x_0,r,angs)

% EXHAUSTIVE EXTENSIVE ALGORITHM
% rc = r^2;
% sum = 0;
% for x = max(1,x_0-r):min(size(imagen,2),x_0+r)
%     for y = max(1,y_0-r):min(size(imagen,1),y_0+r)
%         if abs((x-x_0)^2+(y-y_0)^2-rc)<2
%             sum = sum + imagen(y,x);
%         end
%     end
% end
% sum = sum/r;

% LIGHT ALGORITHM
sum = 0;
for ang = angs
    y = round(y_0-cos(ang)*r);
    x = round(x_0+sin(ang)*r);
    if y<1
        y = 1;
    elseif y>size(imagen,1)
        y = size(imagen,1);
    end
    if x<1
        x = 1;
    elseif x>size(imagen,2)
        x = size(imagen,2);
    end
    sum = sum + imagen(y,x);
end


 function y = jinc(x)
%function y = jinc(x)
% jinc(x) = J_1(pi x) / (2 x),
% where J_1 is Bessel function of the first kind of order 1.
% This is the 2D Fourier transform of a disk of diameter 1,
% so its DC value is the area of that disk which is pi/4.
% Equivalently it is the Hankel transform of rect = @(r) abs(r) < 1/2;
% Jeff Fessler, University of Michigan

x = abs(x); % kludge for bessel with negative arguments, perhaps not needed
y = pi/4 + 0 * x; % jinc(0) = pi/4
ig = x ~= 0;
y(ig) = besselj(1, pi * x(ig)) ./ (2 * x(ig));


function Y = symNormalRnd(M, S, sz)

% Generates independent symmetric matrices
% according to a matrix-variate symmetric normal distribution.
%
% Y = symNormalRnd(M, S, sz)
%
%
% Input:
%   M       pxp mean symmetric matrix
%   S       qxq covariance matrix
%   sz      size of the array (e.g. number of samples)
%
% Output:
%   Y       pxpx[sz] array of random symmetric matrices.
%
% Copyright: Armin Schwartzman, 2009
%

% HISTORY:
%   2008.12.30 ASH (armins@hsph.harvard.edu) wrote it.

% Check inputs
if (size(M,1) ~= size(M,2)),
    error('Wrong input format');
end
if (size(S,1) ~= size(S,2)),
    error('Wrong input format');
end
p = size(M,1);
q = p*(p+1)/2;
if (size(S,1) ~= q),
    error('Wrong input format');
end
if ~exist('sz'),
    sz = 1;
end

%-----------------------------------------------------------------
% Generate multivariate normal
N = prod(sz);       % number of samples
z = mvnrnd(zeros(1,q), S, N);   % size Nxq
z = reshape(z', [q sz]);        % size qxN
Y = repmat(M, [1 1 sz]) + vecdinv(z);

return


function [ average, sd, covc ] = covariance ( dim_num, point_num, x )

%*****************************************************************************80
%
%% COVARIANCE does a covariance calculation for IHS solutions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 April 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer POINT_NUM, the number of points to be generated.
%
%    Input, integer X(DIM_NUM,POINT_NUM), the points.
%
%    Output, real AVERAGE, the average minimum distance.
%
%    Output, real SD, the standard deviation of the minimum distances.
%
%    Output, real COVC, the covariance of the minimum distances.
%

%
%  Set up the distance matrix.
%
  for i = 1 : point_num
    mindist(i) = realmax;
    for j = 1 : point_num
      if ( i ~= j )
        vec(1:dim_num) = x(1:dim_num,i) - x(1:dim_num,j);
        dist = sqrt ( vec(1:dim_num) * vec(1:dim_num)' );
        if ( dist < mindist(i) )
          mindist(i) = dist;
        end
      end
    end
  end
%
%  Find the average minimum distance.
%
  average = sum ( mindist(1:point_num) ) / point_num;
%
%  Compute the standard deviation of the distances.
%
  sd = std ( mindist(1:point_num) );
%
%  Compute the covariance.
%
  covc = sd / average;

  return
end


function [ssarr, l_arr, cu_arr]=refparams_vecgsm(org,subands,M)

%This function computes the parameters of the reference image. This is
%called by vifvec.m.

for i=1:length(subands);
    sub=subands(i);
    y=org{sub};

    sizey=floor(size(y)./M)*M; % crop  to exact multiple size
    y=y(1:sizey(1),1:sizey(2));

    % Collect MxM blocks. Rearrange each block into an
    % M^2 dimensional vector and collect all such vectors.
    % Collece ALL possible MXM blocks (even those overlapping) from the subband
    temp=[];
    for j=1:M
        for k=1:M
            temp=cat(1,temp,reshape(y(k:end-(M-k), j:end-(M-j)),1,[]));
        end
    end

    % estimate mean and covariance
    mcu=mean(temp')';
    cu=((temp-repmat(mcu,1,size(temp,2)))*(temp-repmat(mcu,1,size(temp,2)))')./size(temp,2); % covariance matrix for U

    % Collect MxM blocks as above. Use ONLY non-overlapping blocks to
    % calculate the S field
    temp=[];
    for j=1:M
        for k=1:M
            temp=cat(1,temp,reshape(y(k:M:end, j:M:end),1,[]));
        end
    end

    % Calculate the S field
    ss=(inv(cu)*temp);
    ss=sum(ss.*temp)./(M*M);
    ss=reshape(ss,sizey/M);

    % Eigen-decomposition
    [v,d]=eig(cu);
    l_arr(sub,:)=diag(d)';

    % rearrange for output
    ssarr{sub}=ss;
    temp=0;
    d=diag(d);
    cu_arr{sub}=cu;
end


function res = bhp(im,thresh,n)

% inputs
% im is the fourier transform of the image
% thresh is the cutoff circle radius

%outputs
% res is the filtered image

[r,c]=size(im);
d0=thresh;

d=zeros(r,c);
h=zeros(r,c);

for i=1:r
    for j=1:c
     d(i,j)=  sqrt( (i-(r/2))^2 + (j-(c/2))^2);
    end
end

for i=1:r
    for j=1:c
      h(i,j)=  1 / (1+ (d0/d(i,j))^(2*n) ) ;
    end
end

for i=1:r
    for j=1:c
    res(i,j)=(h(i,j))*im(i,j);

    end
end


function value = p27_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P27_F evaluates the integrand for problem 27.
%
%  Dimension:
%
%    N arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integral Parameters:
%
%    The integral depends on a parameter R and vector C(1:N).
%
%    R defaults to 0.3.
%
%    The reference suggests choosing C at random in [0,1]
%    and then multiplying by the normalizing factor (25/N).
%    C(1:N) defaults to 1/N.
%
%    To get or set R, call P27_R8.
%    To get or set C, call P27_R8VEC.
%
%  Integrand:
%
%    cos ( 2 * pi * R + sum ( c(1:n) * x(1:n) ) )
%
%  Exact Integral:
%
%    2^N * cos ( 2 * pi * R + 0.5 * sum ( c(1:n) ) )
%      * product ( sin ( 0.5 * c(1:n) ) / c(1:n) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Alan Genz,
%    [Integral #1]
%    A Package for Testing Multiple Integration Subroutines,
%    in Numerical Integration: Recent Developments, Software
%    and Applications,
%    edited by Patrick Keast and Graeme Fairweather,
%    D Reidel, 1987, pages 337-340,
%    LC: QA299.3.N38.
%
%    Thomas Patterson,
%    [Integral #5],
%    On the Construction of a Practical Ermakov-Zolotukhin
%    Multiple Integrator,
%    in Numerical Integration: Recent Developments, Software
%    and Applications,
%    edited by Patrick Keast and Graeme Fairweather,
%    D. Reidel, 1987, pages 269-290,
%    LC: QA299.3.N38.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  r = 0.0;
  r = p27_r8 ( 'G', 'R', r );

  c = [];
  c = p27_r8vec ( 'G', 'C', dim_num, c );

  value(1:point_num) = 0.0;

  for point = 1 : point_num

    arg = 2.0 * pi * r + c(1:dim_num,1)' * x(1:dim_num,point);
    value(point) = cos ( arg );

  end

  p27_i4 ( 'I', '#', point_num );

  return
end


function zeta_test ( )

%*****************************************************************************80
%
%% ZETA_TEST tests ZETA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 October 2005
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ZETA_TEST\n' );
  fprintf ( 1, '  ZETA evaluates the Riemann Zeta function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       N                 Zeta                  Zeta\n' );
  fprintf ( 1, '                        exact              computed\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, z1 ] = zeta_values ( n_data );

    if ( n_data == 0 )
      break
    end

    z2 = zeta ( n );

    fprintf ( 1, '  %6d  %20e  %20e\n', n, z1, z2 );

  end

  return
end


function y = sigmoidabTransform(x, transform, varargin)

% SIGMOIDABTRANSFORM Constrains a parameter to be between A and B
% by a scaled logistic sigmoid function.
%
% FORMAT
%
% DESC contains commands to constrain parameters to be between A
% and B via the sigmoid function, y=A+(B-A)/(1+exp(-x)).
%
% ARG x : input argument.
%
% ARG transform : type of transform, 'atox' maps a value into
% the transformed space (i.e. makes it between A and B). 'xtoa'
% maps the parameter back from transformed space to the original
% space. 'gradfact' gives the factor needed to correct gradients
% with respect to the transformed parameter, that is, it gives
% the gradient dx/da where x is the transformed parameter and a
% is the untransformed parameter.
%
% ARG transformsettings (first element of varargin): vector [A B]
% giving the minimum and maximum values A and B for the transformed
% parameter. If not given, assume A=0 and B=1 as in the function
% sigmoidTransform.
%
% OUTPUT y : return argument.
%
% SEEALSO : negLogLogitTransform, expTransform
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006, 2007
%
% COPYRIGHT : Jaakko Peltonen, 2011
%
% COPYRIGHT : Antti Honkela, 2012

% OPTIMI

% Get the transformation settings from the varargin structure
if ~isempty(varargin),
  transformsettings=varargin{1};
else
  transformsettings=[];
end;

identitymode=0;
if identitymode==1,
  % DEBUG ONLY, turns transformation to identity!
  fprintf(1,'Using identity mode for transform %s of parameter value %f\n',transform, x)
  switch transform
   case 'atox'
    y=x;
   case 'xtoa'
    y=x;
   case 'gradfact'
    y=0*x+1;
  end
else
  % normal mode, logistic sigmoid between A and B

  if isempty(transformsettings),
    warning(sprintf('sigmoidabTransform(%f,%s) called without transformation settings\n',x,transform));
    A=0; B=1;
  else
    A=transformsettings(1);
    B=transformsettings(2);
  end;

  limVal = 36;
  minval_sigmoid=A+(B-A)*eps;
  maxval_sigmoid=A+(B-A)*(1-eps);

  y = zeros(size(x));
  switch transform
   case 'atox'
    %x
    %A
    %B
    %fprintf(1,'transforming x %f, A %f, B %f\n',x,A,B);

    I1 = x < -limVal;
    y(I1) = A+(B-A)*eps;

    I2 = x > limVal;
    y(I2) = A+(B-A)*(1-eps);

    I3 = ~I1 & ~I2;
    y(I3) = A+(B-A)*sigmoid(x(I3));

   case 'xtoa'
    %  [x A B]
    %fprintf(1,'inverse-transforming x %f, A %f, B %f\n',x,A,B);
    I1 = x<=minval_sigmoid;
    y(I1) = -limVal;

    I2 = x>=maxval_sigmoid;
    y(I2) = limVal;

    I3 = ~I1 & ~I2;
    y(I3) = invSigmoid((x(I3)-A)/(B-A));

   case 'gradfact'
    %fprintf(1,'gradfact-transforming x %f, A %f, B %f\n',x,A,B);
    %y = (B-A)*x.*(1-x);
    y = (x-A).*(B-x)/(B-A);
    %y=0*x+1; % DEBUG ONLY, turns factors off!
  end
end;


function w = ymdf_to_weekday_julian2 ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_WEEKDAY_JULIAN2 returns the weekday of a Julian YMDF date.
%
%  Discussion:
%
%    This routine computes the day of the week from the date in
%    the Julian calendar, that is, the calendar in force before the
%    Gregorian calendar, in which every fourth year was a leap year.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Algorithm A,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999, pages 307-308.
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, integer W, the day of the week of the date.
%    The days are numbered from Sunday through Saturday, 1 through 7.
%
  m_prime = mod ( 9 + m, 12 );
  q = floor ( m_prime / 10 );
  z = floor ( ( 13 * m_prime + 2 ) / 5 );
  t = 28 * m_prime + z + d - 365 * q + 59;

  c = i4_wrap ( t, 1, 7 );

  y_prime = y - q;
  v = floor ( y / 4 ) - floor ( y_prime / 4 );
  p = y + floor ( y / 4 ) + 4 - v;
  n = 7 - mod ( p, 7 );

  w = i4_wrap ( c + 1 - n, 1, 7 );

  return
end


function value = c8_log ( z )

%*****************************************************************************80
%
%% C8_LOG evaluates the logarithm of a C8.
%
%  Discussion:
%
%    A C8 is a complex value.
%
%    Here we use the relationship:
%
%      C8_LOG ( Z ) = LOG ( MAG ( Z ) ) + i * ARG ( Z )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, complex Z, the argument.
%
%    Output, complex VALUE, the function value.
%
  arg = c8_arg ( z );
  mag = c8_mag ( z );

  value = log ( mag ) + arg * i;

  return
end


function KeplerUniversalVsSTK()
%% Constants
mu = 398600.4418;

%% Polar GEO Altitude Orbit Propagation Comparison
load('PolarGEO.mat');
r0 = repmat(PosVel(1,1:3)',[1 size(PosVel,1)]);
v0 = repmat(PosVel(1,4:6)',[1 size(PosVel,1)]);
[r,v] = keplerUniversal(r0,v0,0:60:(size(PosVel,1)-1)*60,mu);
PlotStateVectorErrors(r,v,PosVel,0:60:(size(PosVel,1)-1)*60,'GEO Altitude, Polar Inclination Propagation');

%% LEO Orbit Propagation Comparison
load('InclinedLEO.mat');
r0 = repmat(PosVel(1,1:3)',[1 size(PosVel,1)]);
v0 = repmat(PosVel(1,4:6)',[1 size(PosVel,1)]);
[r,v] = keplerUniversal(r0,v0,0:60:(size(PosVel,1)-1)*60,mu);
PlotStateVectorErrors(r,v,PosVel,0:60:(size(PosVel,1)-1)*60,'LEO Inclined Propagation');

%% Hyperbolic LEO Altitude Orbit Propagtion Comparison
load('HyperbolicLEO.mat');
r0 = repmat(PosVel(1,1:3)',[1 size(PosVel,1)]);
v0 = repmat(PosVel(1,4:6)',[1 size(PosVel,1)]);
[r,v] = keplerUniversal(r0,v0,0:60:(size(PosVel,1)-1)*60,mu);
PlotStateVectorErrors(r,v,PosVel,0:60:(size(PosVel,1)-1)*60,'Hyperbolic Orbit Propagation');

%% Parabolic LEO Altitude Orbit Propagation comparison
load('ParabolicLEO.mat');
r0 = repmat(PosVel(1,1:3)',[1 size(PosVel,1)]);
v0 = repmat(PosVel(1,4:6)',[1 size(PosVel,1)]);
[r,v] = keplerUniversal(r0,v0,0:60:(size(PosVel,1)-1)*60,mu);
PlotStateVectorErrors(r,v,PosVel,0:60:(size(PosVel,1)-1)*60,'Parabolic Orbit Propagation');

end

function PlotStateVectorErrors(r,v,PosVel,t,titleText)
MaxPositionError = abs(r-PosVel(:,1:3)');
MaxVelocityError = abs(v-PosVel(:,4:6)');
figure('color',[1 1 1]);
subplot(4,1,1); plot(t./60,MaxPositionError); title(titleText); ylabel('Pos Err (km)');
subplot(4,1,2); plot(t./60,MaxVelocityError); ylabel('Vel Err (km/s)');
subplot(4,1,3); plot(t./60,abs(sqrt(sum(r.^2,1)) - sqrt(sum(PosVel(:,1:3)'.^2,1)))); ylabel('Pos Mag Err (km)'); xlabel('Time (m)');
subplot(4,1,4); plot(t./60,abs(sqrt(sum(v.^2,1)) - sqrt(sum(PosVel(:,4:6)'.^2,1)))); ylabel('Vel Mag Err (km/s)'); xlabel('Time (m)');
end


function eg2OC_BVP
%EG2OC_BVP    Example 2 of optimal control tutorial.
%    This example is from D.E.Kirk's Optimal control theory: an introduction
%    example 6.2-2 on page 338-339
%    The problem is reformulated as a boundary value problem (BVP)
%    and solved with bvp4c solver

% Initial guess for the solution
solinit = bvpinit(linspace(0,0.78,50), [0 0 0.5 0.5]);
options = bvpset('Stats','on','RelTol',1e-1);
global R;
R = 0.1;
sol = bvp4c(@BVP_ode, @BVP_bc, solinit, options);
t = sol.x;
y = sol.y;

% Calculate u(t) from x1,x2,p1,p2
ut = (y(3,:).*(y(1,:) + 1/4))/(2*0.1);
n = length(t);
% Calculate the cost
J = 0.78*(y(1,:)*y(1,:)' + y(2,:)*y(2,:)' + ut*ut'*0.1)/n;

figure(1);
plot(t, y(1:2,:)','-');hold on;
plot(t,ut', 'r:')
text(.2,0.08,'x_1(t)');
text(.3,-.1,'x_2(t)');
text(.2,.4, 'u(t)');
s = strcat('Final cost is: J=',num2str(J));
text(.4,1,s);
xlabel('time');
ylabel('states');
hold off;
print -djpeg90 -r300 eg2_bvp.jpg

%------------------------------------------------
% ODE's for states and costates
function dydt = BVP_ode(t,y)
global R;
t1 = y(1)+.25;
t2 = y(2)+.5;
t3 = exp(25*y(1)/(y(2)+2));
t4 = 50/(y(1)+2)^2;
u =  y(3)*t1/(2*R);

dydt = [-2*t1+t2*t3-t2*u
        0.5-y(2)-t2*t3
        -2*y(1)+2*y(3)-y(3)*t2*t4*t3+y(3)*u+y(4)*t2*t4*t3
        -2*y(2)-y(3)*t3+y(4)*(1+t3)];

% -----------------------------------------------
% The boundary conditions:
% x1(0) = 0.05, x2(0) = 0, tf = 0.78, p1(tf) = 0, p2(tf) = 0;
function res = BVP_bc(ya,yb)
res = [ ya(1) - 0.05
        ya(2) - 0
        yb(3) - 0
        yb(4) - 0 ];


%EUL2TR Convert Euler angles to homogeneous transform
%
% T = EUL2TR(PHI, THETA, PSI, OPTIONS) is an SE(3) homogeneous
% transformation matrix (4x4) with zero translation and rotation equivalent
% to the specified Euler angles. These correspond to rotations about the Z,
% Y, Z axes respectively. If PHI, THETA, PSI are column vectors (Nx1) then
% they are assumed to represent a trajectory and R is a three-dimensional
% matrix (4x4xN), where the last index corresponds to rows of PHI, THETA,
% PSI.
%
% R = EUL2R(EUL, OPTIONS) as above but the Euler angles are taken from the
% vector (1x3)  EUL = [PHI THETA PSI]. If EUL is a matrix (Nx3) then R is a
% three-dimensional matrix (4x4xN), where the last index corresponds to
% rows of RPY which are assumed to be [PHI,THETA,PSI].
%
% Options::
%  'deg'      Angles given in degrees (radians default)
%
% Note::
% - The vectors PHI, THETA, PSI must be of the same length.
% - The translational part is zero.
%
% See also EUL2R, RPY2TR, TR2EUL, SE3.eul.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

function T = eul2tr(phi, varargin)

    R = eul2r(phi, varargin{:});
    T = r2t(R);


function f = tt_loglikelihood(X,M)
%TT_LOGLIKELIHOOD Compute log-likelihood of data X with model M.
%
%   F = TT_LOGLIKELIHOOD(X,M) computes the log-likelihood of model M given
%   data X, where M is a ktensor and X is a tensor or sptensor.
%   Specifically, F = - (sum_i m_i - x_i * log_i) where i is a multiindex
%   across all tensor dimensions.
%
%   See also cp_apr, tensor, sptensor, ktensor.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

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

N = ndims(X);

if ~isa(M, 'ktensor')
    error('M must be a ktensor');
end

M = normalize(M,1,1);

if isa(X, 'sptensor')
    xsubs = X.subs;
    A = M.U{1}(xsubs(:,1),:);
    for n = 2:N
       A = A .* M.U{n}(xsubs(:,n),:);
    end
    f = sum(X.vals .* log(sum(A,2))) - sum(sum(M.U{1}));
else
    f = sum(sum(double(tenmat(X,1)) .* log(double(tenmat(M,1))))) - sum(sum(M.U{1}));
end


function [ n_data, x, fx ] = elliptic_ka_values ( n_data )

%*****************************************************************************80
%
%% ELLIPTIC_KA_VALUES returns values of the complete elliptic integral K(ALPHA).
%
%  Discussion:
%
%    This is one form of what is sometimes called the complete elliptic integral
%    of the first kind.
%
%    The function is defined by the formula:
%
%      K(ALPHA) = integral ( 0 <= T <= PI/2 )
%        dT / sqrt ( 1 - sin ( ALPHA )^2 * sin ( T )^2 )
%
%    In Mathematica, the function can be evaluated by:
%
%      EllipticK[(Sin[alpha*Pi/180])^2]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function, measured
%    in degrees.
%
%    Output, real FX, the value of the function.
%
  n_max = 18;

  fx_vec = [ ...
     0.1570796326794897E+01, ...
     0.1573792130924768E+01, ...
     0.1582842804338351E+01, ...
     0.1598142002112540E+01, ...
     0.1620025899124204E+01, ...
     0.1648995218478530E+01, ...
     0.1685750354812596E+01, ...
     0.1731245175657058E+01, ...
     0.1786769134885021E+01, ...
     0.1854074677301372E+01, ...
     0.1935581096004722E+01, ...
     0.2034715312185791E+01, ...
     0.2156515647499643E+01, ...
     0.2308786798167196E+01, ...
     0.2504550079001634E+01, ...
     0.2768063145368768E+01, ...
     0.3153385251887839E+01, ...
     0.3831741999784146E+01 ];

  x_vec = [ ...
      0.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     15.0E+00, ...
     20.0E+00, ...
     25.0E+00, ...
     30.0E+00, ...
     35.0E+00, ...
     40.0E+00, ...
     45.0E+00, ...
     50.0E+00, ...
     55.0E+00, ...
     60.0E+00, ...
     65.0E+00, ...
     70.0E+00, ...
     75.0E+00, ...
     80.0E+00, ...
     85.0E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function y = entr( x )

%ENTR   Scalar entropy.
%   ENTR(X) returns an array of the same size as X with the unnormalized
%   entropy function applied to each element:
%                { -X.*LOG(X) if X > 0,
%      ENTR(X) = { 0          if X == 0,
%                { -Inf       otherwise.
%   If X is a vector representing a discrete probability distribution, then
%   SUM(ENTR(X)) returns its entropy.
%
%   Disciplined convex programming information:
%       ENTR(X) is concave and nonmonotonic in X. Thus when used in CVX
%       expressions, X must be real and affine. Its use will effectively
%       constrain X to be nonnegative: there is no need to add an
%       additional X >= 0 to your model in order to enforce this.

error(nargchk(1,1,nargin));
cvx_expert_check( 'entr', x );
y = -rel_entr( x, 1 );

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function r8po_print ( n, a, title )

%*****************************************************************************80
%
%% R8PO_PRINT prints a R8PO matrix.
%
%  Discussion:
%
%    The R8PO storage format is appropriate for a symmetric positive definite
%    matrix and its inverse.  (The Cholesky factor of an SPO matrix is an
%    upper triangular matrix, so it will be in R8GE storage format.)
%
%    Only the diagonal and upper triangle of the square array are used.
%    This same storage scheme is used when the matrix is factored by
%    R8PO_FA, or inverted by R8PO_INVERSE.  For clarity, the lower triangle
%    is set to zero.
%
%  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, real A(N,N), the R8PO matrix.
%
%    Input, string TITLE, a title to be printed.
%
  r8po_print_some ( n, a, 1, 1, n, n, title );

  return
end


function [x] = gels(a,b,param)
%
% GELS   Solves an overdetermined or underdetermined linear problem.
%
%    [X] = GELS(A,B) same as [X] = GELS(A,B,'default')
%
%    [X] = GELS(A,B,PARAM) if A is m-by-n and m > n then GELS computes the solution of the
%    linear least squares problem
%            min || A * X - B ||_F ,
%    if A is m-by-n and m < n then GELS computes the solution of the minimum norm problem
%            min || X ||_F s.t. A*X = B
%    PARAM controls the algorithm used:
%
%       'dc'
%             performs SVD factorization of A using the divide and conquer
%             method then solve the undetermined or overdetermined problem.
%             calls LAPACK routine _GELSD
%
%       'svd'
%             performs SVD factorization of A using QR iterations then solve the
%             undetermined or overdetermined problem.
%             calls LAPACK routine _GELSS
%
%       'y'
%             performs QR factorization of A then solve the undetermined or
%             overdetermined problem.
%             calls LAPACK routine _GELSY
%
%       'default'
%             (default)
%             performs QR factorization of A without pivoting, then solve the
%             undetermined or overdetermined problem.  The fact that there is no
%             pivoting in the QR factorization means that A(:,1:min(size(A))
%             needs to be full rank.
%             calls LAPACK routine _GELS
%
%    When A is m-by-n with m > n then [X] = GELS(A,B) is the same as X=A\B.
%
%    When A is m-by-n with m < n  then [X] = GELS(A,B,PARAM) is _not_ the same as
%    X=A\B. [X] = GELS(A,B,PARAM) solves the problem
%    (*)    min_X || X ||_F s.t. AX=B
%    while X=A\B solves AX=B by first performing a QR factorization of A
%          [Q,R,E] = QR(A)
%    then X is obtained via
%          X = E(:,1:m)*(R(:,1:m)\(Q'*b))]
%	The \ method is faster than GELS, X is such that AX=B but X is not (in the
%	general case) the solution of (*).
%
%    See also: \, MLDIVIDE.
%
	x=[];

	if (nargout~=1),
		disp('Wrong number of output parameters');
		return;
	end;

	if (nargin~=2 && nargin~=3),
		disp('Wrong number of input parameters');
		return;
	end;

	%Do all the boring checking
	if (~isnumeric(a)),
		disp('The matrix is not composed of numeric values.');
		return;
	elseif (isinteger(a)),
		a=double(a);
	end;

	if ((nargin == 2)||(strcmp(param,'default'))),
		trans = 'N';
		[x,info] = gels_2(trans,a,b);
	else
		if (strcmp(param,'dc')),
			rcond = -1; % machine precision
			[x,rank,info] = gelsd(a,b,rcond);
		elseif (strcmp(param,'svd')),
			rcond = -1; % machine precision
			[x,rank,info] = gelss(a,b,rcond);
		elseif (strcmp(param,'y')),
			rcond = eps; % machine precision
			[x,rank,info] = gelsy(a,b,rcond);
		else
			disp('Type of algorithm not correct. Please choose dc, svd, y, or default.');
			return;
		end;
	end;

	%Do post-call boring checking
	if (info~=0),
		disp('Resolution failed, no solution returned!');
		x=[];
		return;
	end;


function [sx, dsdx, dsdp] = VBA_sparsifyPrior (x, logExponent, smoothness)
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [sx, dsdx, dsdp] = VBA_sparsifyPrior (x, varargin)
% parameter transformation that emulates Laplace priors (L1-norm)
%
% IN:
%   - x: input value to be transformed
%   - logExponent: optional sparsity parameter. If P = 0, then the mapping
%     is regular and no sparsity is emulated. If P=log(2), then the mapping
%     emulates L1-norm like priors.
%   - smoothness: optional smoothness parameter of the sign approximation
%     (default = 1)
%
% In the general case, you won't have to change any of the optional
% parameters. Those options allows to explore the properties of the
% transformation and should not be changed.
%
% OUT:
%   - sx: transformed value
%   - dsdx: gradient of sx wrt x
%   - dsdP: gradient of sx wrt logExponent parameter
%
% /////////////////////////////////////////////////////////////////////////

% check parameters
% =========================================================================
if nargin < 2
    logExponent = log(2);
end

if nargin < 3
    smoothness = []; % use VBA_sign default
end

% shortcuts
% =========================================================================
[signX, d_signX] = VBA_sign(x, smoothness);
[absX, d_absX] = VBA_abs (x);
exponent = exp(logExponent);

% compute transformation
% =========================================================================

sx = signX .* (absX .^ exponent);

% derivatives
% =========================================================================

% wrt to x
% -------------------------------------------------------------------------
if nargout < 2
    return;
end

dsdx = d_signX .* (absX .^ exponent) ...
     + exponent .* absX .^ (exponent - 1) .* signX .* d_absX;

% wrt to logExponent
% -------------------------------------------------------------------------
if nargout < 3
    return;
end

dsdp = (absX .^ exponent) .* signX .* log(absX) .* exponent;


function[I]=besselitilde(varargin)
%BESSELITILDE  I-type Bessel function after factoring off exponential growth.
%
%   ITILDE=BESSELITILDE(NU,Z) returns the modified Bessel function of the
%   first kind of order NU at argument Z, after factoring off the
%   asymptotic behavior of EXP(Z).
%
%   BESSELITILDE is useful for products of modified Bessel functions in
%   which the exponential behaviors cancel, but that cannot be evaluated
%   directly because of numerical overflow.
%
%   BESSELITILDE(NU,Z,N) use a summation truncated at N terms. The default
%   behavior uses N=30 and is highly accurate.
%
%   See 10.40.1 of https://dlmf.nist.gov/10.40.
%
%   This is low-level code used by WINDTRANS using an algorithm described
%   in Lilly and Elipot (2021).
%
%   See also BESSELKTILDE.
%
%   'besselitilde --t' runs a test.
%
%   Usage: I=besselitilde(nu,z);
%          I=besselitilde(nu,z,nterms);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2019--2021 J.M. Lilly --- type 'help jlab_license' for details

if strcmp(varargin{1}, '--t')
    besselitilde_test,return
end

nu=varargin{1};
z=varargin{2};
nterms=30;
if nargin==3
    nterms=varargin{3};
end

sizez=size(z);
z=z(:);
%let's sum over the third dimensions
zk=vrep(-z,nterms,2);  %this makes the alternating sign
zk(:,1)=1;
zk=cumprod(zk,2);

k=(0:nterms-1)';
ak=(4*nu.^2-(2*k-1).^2);
ak(1)=1;
ak=cumprod(ak,1);
ak=frac(ak,factorial(k).*8.^k);
if any(~isfinite(ak))
    ak=ak(1:find(~isfinite(ak),1,'first')-1);
end
zk=zk(:,1:length(ak));

I=frac(1,sqrt(2*z*pi)).*((1./zk)*ak);
I=reshape(I,sizez);
%nterms

% %let's sum over the third dimensions
% zk=vrep(-z,nterms,3);  %this makes the alternating sign
% zk(:,:,1)=1;
% zk=cumprod(zk,3);
%
% k=(0:nterms-1)';
% ak=(4*nu.^2-(2*k-1).^2);
% ak(1)=1;
% ak=cumprod(ak,1);
% ak=frac(ak,factorial(k).*8.^k);
% if any(~isfinite(ak))
%     ak=ak(1:find(~isfinite(ak),1,'first')-1);
% end
% zk=zk(:,:,1:length(ak));
%
% ak=vrep(permute(ak,[3 2 1]),size(z),[1 2]);
%
% I=frac(1,sqrt(2*z*pi)).*sum(frac(ak,zk),3);

function[]=besselitilde_test

for s=[1 -1]
    z=sqrt(s*1i)*[23:0.01:100]';
    %z=[20:0.01:100]';
    [bi0,bi]=vzeros(length(z),2);
    for i=1:2
        bi0(:,i)=exp(-z).*besseli(i-1,z);
        bi(:,i)=besselitilde(i-1,z);
    end

    if s==1
        reporttest('BESSELITILDE for z with phase of pi/4',allall(abs((bi0-bi)./bi)<1e-14))
    else
        reporttest('BESSELITILDE for z with phase of -pi/4',allall(abs((bi0-bi)./bi)<1e-14))
    end

	bi=vzeros(length(z),2);
    for i=1:2
        bi(:,i)=besselitilde(i-1,z,2);
    end

    if s==1
        reporttest('BESSELITILDE 2-term for z with phase of pi/4, order 0 and 1',allall(abs((bi0(:,1:2)-bi)./bi)<1e-3))
    else
        reporttest('BESSELITILDE 2-term for z with phase of -pi/4, order 0 and 1',allall(abs((bi0(:,1:2)-bi)./bi)<1e-3))
    end

    bi0=sqrt(frac(1,2*pi*z)).*(1+frac(1,8*z));
    bi1=sqrt(frac(1,2*pi*z)).*(1-frac(3,8*z));

    if s==1
        reporttest('BESSELITILDE 2-term vs. analytic for z with phase of pi/4',allall(abs(([bi0 bi1]-bi)./bi)<1e-15))
    else
        reporttest('BESSELITILDE 2-term  vs. analytic for z with phase of -pi/4',allall(abs(([bi0 bi1]-bi)./bi)<1e-15))
    end
end

% figure,plot(abs(z),abs(bi-bi0)),ylog

% nu=5;
% z=[0:0.001:100]';
% tic;bi(:,1)=besselitilde(nu,z);toc
% tic;bi(:,2)=sqrt(2*z*pi).*exp(-z).*besseli(nu,z);toc
% figure,plot(z,log10(abs((bi(:,1)-bi(:,2))./bi(:,2))))
% %wow, really excellent
%
%
% tic;bi0=sqrt(2*z*pi).*exp(-z).*besseli(nu,z);toc
% for i=1:50
%     bi(:,i)=besselitilde(nu,z,i);
% end
% bi0=vrep(bi0,size(bi,2),2);
% plot(z,log10(abs((bi-bi0)./bi0)))
%
%


function MS = createMesh3D(varargin)
% MeshStructure = createMesh3D(Nx, Ny, Nz, Width, Height, Depth)
% creates a uniform 3D mesh:
% Nx is the number of cells in x (horizontal) direction
% Ny is the number of cells in y (vertical) direction
% Nz is the number of cells in z (perpendicular) direction
% Lx is the domain length in x direction
% Ly is the domain length in y direction
% Lz is the domain length in z direction
%
% SYNOPSIS:
%   MeshStructure = createMesh3D(Nx, Ny, Nz, Lx, Ly, Lz)
%
% PARAMETERS:
%   Nx: number of cells in the x direction
%   Lx: domain length in x direction
%   Ny: number of cells in the y direction
%   Ly: domain length in y direction
%   Nz: number of cells in the z direction
%   Lz: domain length in z direction
%
% RETURNS:
%   MeshStructure.
%                 dimensions=3 (3D problem)
%                 numbering: shows the indexes of cellsn from left to right
%                 and top to bottom and back to front
%                 cellsize: x, y, and z elements of the cell size =[Lx/Nx,
%                 Ly/Ny, Lz/Nz]
%                 cellcenters.x: location of each cell in the x direction
%                 cellcenters.y: location of each cell in the y direction
%                 cellcenters.z: location of each cell in the z direction
%                 facecenters.x: location of interface between cells in the
%                 x direction
%                 facecenters.y: location of interface between cells in the
%                 y direction
%                 facecenters.z: location of interface between cells in the
%                 z direction
%                 numberofcells: [Nx, Ny, Nz]
%
%
% EXAMPLE:
%   Nx = 2;
%   Lx = 1.0;
%   Ny = 3;
%   Ly = 2.0;
%   Nz = 4;
%   Lz = 3.0;
%   m = createMesh3D(Nx, Ny, Nz, Lx, Ly, Lz);
%   [X, Y, Z] = ndgrid(m.cellcenters.x, m.cellcenters.y, m.cellcenters.z);
%   [Xf, Yf, Zf] = ndgrid(m.facecenters.x, m.facecenters.y, m.facecenters.z);
%   plot3(X(:), Y(:), Z(:), 'or')
%   hold on;
%   plot3(Xf(:), Yf(:), Zf(:), '+b')
%   legend('cell centers', 'cell corners');
%
% SEE ALSO:
%     createMesh1D, createMesh2D, createMeshCylindrical1D, ...
%     createMeshCylindrical2D, createCellVariable, createFaceVariable

% Written by Ali A. Eftekhari
% See the license file

if nargin==6
  % uniform 1D mesh
  Nx=varargin{1};
  Ny=varargin{2};
  Nz=varargin{3};
  Width=varargin{4};
  Height=varargin{5};
  Depth=varargin{6};
  % cell size is dx
  dx = Width/Nx;
  dy = Height/Ny;
  dz = Depth/Nz;
  G=reshape(1:(Nx+2)*(Ny+2)*(Nz+2), Nx+2, Ny+2, Nz+2);
  CellSize.x= dx*ones(Nx+2,1);
  CellSize.y= dy*ones(Ny+2,1);
  CellSize.z= dz*ones(Nz+2,1);
  CellLocation.x= [1:Nx]'*dx-dx/2;
  CellLocation.y= [1:Ny]'*dy-dy/2;
  CellLocation.z= [1:Nz]'*dz-dz/2;
  FaceLocation.x= [0:Nx]'*dx;
  FaceLocation.y= [0:Ny]'*dy;
  FaceLocation.z= [0:Nz]'*dz;
elseif nargin==3
  % nonuniform 1D mesh
  facelocationX=varargin{1};
  facelocationY=varargin{2};
  facelocationZ=varargin{3};
  facelocationX=facelocationX(:);
  facelocationY=facelocationY(:);
  facelocationZ=facelocationZ(:);
  Nx = length(facelocationX)-1;
  Ny = length(facelocationY)-1;
  Nz = length(facelocationZ)-1;
  G=reshape(1:(Nx+2)*(Ny+2)*(Nz+2), Nx+2, Ny+2, Nz+2);
  CellSize.x= [facelocationX(2)-facelocationX(1); ...
    facelocationX(2:end)-facelocationX(1:end-1); ...
    facelocationX(end)-facelocationX(end-1)];
  CellSize.y= [facelocationY(2)-facelocationY(1); ...
    facelocationY(2:end)-facelocationY(1:end-1); ...
    facelocationY(end)-facelocationY(end-1)];
  CellSize.z= [facelocationZ(2)-facelocationZ(1); ...
    facelocationZ(2:end)-facelocationZ(1:end-1); ...
    facelocationZ(end)-facelocationZ(end-1)];
  CellLocation.x= 0.5*(facelocationX(2:end)+facelocationX(1:end-1));
  CellLocation.y= 0.5*(facelocationY(2:end)+facelocationY(1:end-1));
  CellLocation.z= 0.5*(facelocationZ(2:end)+facelocationZ(1:end-1));
  FaceLocation.x= facelocationX;
  FaceLocation.y= facelocationY;
  FaceLocation.z= facelocationZ;
end
c=G([1,end], [1,end], [1, end]);
e1=G([1, end], [1, end], 2:Nz+1);
e2=G([1, end], 2:Ny+1, [1, end]);
e3=G(2:Nx+1, [1, end], [1, end]);
MS=MeshStructure(3, ...
  [Nx,Ny, Nz], ...
  CellSize, ...
  CellLocation, ...
  FaceLocation, ...
  c(:), ...
  [e1(:); e2(:); e3(:)]);


function t = traceProduct(A, B)

% TRACEPRODUCT Returns the trace of the product of two matrices.
% FORMAT
% DESC returns the trace of the product of two matrices, tr(A*B).
% ARG A : the first matrix in the product.
% ARG B : the second matrix in the product.
% RETURN t : the trace of the product.
%
% SEEALSO : trace
%
% COPYRIGHT : Neil D. Lawrence, 2004

% NDLUTIL

t = sum(sum(A.*B'));


function value = year_is_embolismic_hebrew ( y )

%*****************************************************************************80
%
%% YEAR_IS_EMBOLISMIC_HEBREW returns TRUE if the Hebrew year was embolismic.
%
%  Discussion:
%
%    In a 19 year cycle, there are 7 embolismic years.  During these years,
%    an extra month, "Adar II", (sometimes called "Veadar") is inserted after
%    the month of Adar.  Nonembolismic years are called "common" years.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 September 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year to be checked.
%
%    Output, logical VALUE, TRUE if the year was embolismic.
%
  if ( 12 <= i4_modp ( 7 * y + 13, 19 ) )
    value = 1;
  else
    value = 0;
  end

  return
end


classdef distance

    methods(Static)

        function result = pairwise_distances(X, distance)
            % Returns the pairwise distance matrix
            if (nargin < 2)
                distance = 'euclidean';
            end
            % Pair wise distance matrix (symmetric)
            result  =  squareform(pdist(X', distance));
        end

        function result = sqrd_l2_distances_cw(X)
            % The vectors are stored column wise.
            % Number of vectors = number of columns
            result = spx.commons.distance.pairwise_x_y(X);
        end

        function result = sqrd_l2_distances_rw(X)
            % The vectors are stored row wise.
            % Number of vectors = number of rows
            result = spx.commons.distance.pairwise_x_y(X');
        end

        function [ distance_matrix ] = pairwise_x_y( X, Y )
            % Computes pairwise distance squared between vectors in X and Y
            % c.f. pdist built-in function
            % We assume that vectors are column vectors
            % Dimension of space to which the vectors belong
            %N = size(X,1);
            % Number of vectors in X
            %nx = size(X,2);
            % Number of vectors in Y
            %ny = size(Y,2);
            % The result
            % distance_matrix(i,j) = distance (x_i, y_j)
            if nargin < 2
                Y = X;
            end
            distance_matrix = bsxfun(@plus,dot(X,X,1)',dot(Y,Y,1))-2*(X'*Y);
        end

    end

end


function ellipse_t = fit_ellipse( x,y,axis_handle )
%
% fit_ellipse - finds the best fit to an ellipse for the given set of points.
%
% Format:   ellipse_t = fit_ellipse( x,y,axis_handle )
%
% Input:    x,y         - a set of points in 2 column vectors. AT LEAST 5 points are needed !
%           axis_handle - optional. a handle to an axis, at which the estimated ellipse
%                         will be drawn along with it's axes
%
% Output:   ellipse_t - structure that defines the best fit to an ellipse
%                       a           - sub axis (radius) of the X axis of the non-tilt ellipse
%                       b           - sub axis (radius) of the Y axis of the non-tilt ellipse
%                       phi         - orientation in radians of the ellipse (tilt)
%                       X0          - center at the X axis of the non-tilt ellipse
%                       Y0          - center at the Y axis of the non-tilt ellipse
%                       X0_in       - center at the X axis of the tilted ellipse
%                       Y0_in       - center at the Y axis of the tilted ellipse
%                       long_axis   - size of the long axis of the ellipse
%                       short_axis  - size of the short axis of the ellipse
%                       status      - status of detection of an ellipse
%
% Note:     if an ellipse was not detected (but a parabola or hyperbola), then
%           an empty structure is returned

% =====================================================================================
%                  Ellipse Fit using Least Squares criterion
% =====================================================================================
% We will try to fit the best ellipse to the given measurements. the mathematical
% representation of use will be the CONIC Equation of the Ellipse which is:
%
%    Ellipse = a*x^2 + b*x*y + c*y^2 + d*x + e*y + f = 0
%
% The fit-estimation method of use is the Least Squares method (without any weights)
% The estimator is extracted from the following equations:
%
%    g(x,y;A) := a*x^2 + b*x*y + c*y^2 + d*x + e*y = f
%
%    where:
%       A   - is the vector of parameters to be estimated (a,b,c,d,e)
%       x,y - is a single measurement
%
% We will define the cost function to be:
%
%   Cost(A) := (g_c(x_c,y_c;A)-f_c)'*(g_c(x_c,y_c;A)-f_c)
%            = (X*A+f_c)'*(X*A+f_c)
%            = A'*X'*X*A + 2*f_c'*X*A + N*f^2
%
%   where:
%       g_c(x_c,y_c;A) - vector function of ALL the measurements
%                        each element of g_c() is g(x,y;A)
%       X              - a matrix of the form: [x_c.^2, x_c.*y_c, y_c.^2, x_c, y_c ]
%       f_c            - is actually defined as ones(length(f),1)*f
%
% Derivation of the Cost function with respect to the vector of parameters "A" yields:
%
%   A'*X'*X = -f_c'*X = -f*ones(1,length(f_c))*X = -f*sum(X)
%
% Which yields the estimator:
%
%       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%       |  A_least_squares = -f*sum(X)/(X'*X) ->(normalize by -f) = sum(X)/(X'*X)  |
%       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%
% (We will normalize the variables by (-f) since "f" is unknown and can be accounted for later on)
%
% NOW, all that is left to do is to extract the parameters from the Conic Equation.
% We will deal the vector A into the variables: (A,B,C,D,E) and assume F = -1;
%
%    Recall the conic representation of an ellipse:
%
%       A*x^2 + B*x*y + C*y^2 + D*x + E*y + F = 0
%
% We will check if the ellipse has a tilt (=orientation). The orientation is present
% if the coefficient of the term "x*y" is not zero. If so, we first need to remove the
% tilt of the ellipse.
%
% If the parameter "B" is not equal to zero, then we have an orientation (tilt) to the ellipse.
% we will remove the tilt of the ellipse so as to remain with a conic representation of an
% ellipse without a tilt, for which the math is more simple:
%
% Non tilt conic rep.:  A`*x^2 + C`*y^2 + D`*x + E`*y + F` = 0
%
% We will remove the orientation using the following substitution:
%
%   Replace x with cx+sy and y with -sx+cy such that the conic representation is:
%
%   A(cx+sy)^2 + B(cx+sy)(-sx+cy) + C(-sx+cy)^2 + D(cx+sy) + E(-sx+cy) + F = 0
%
%   where:      c = cos(phi)    ,   s = sin(phi)
%
%   and simplify...
%
%       x^2(A*c^2 - Bcs + Cs^2) + xy(2A*cs +(c^2-s^2)B -2Ccs) + ...
%           y^2(As^2 + Bcs + Cc^2) + x(Dc-Es) + y(Ds+Ec) + F = 0
%
%   The orientation is easily found by the condition of (B_new=0) which results in:
%
%   2A*cs +(c^2-s^2)B -2Ccs = 0  ==> phi = 1/2 * atan( b/(c-a) )
%
%   Now the constants   c=cos(phi)  and  s=sin(phi)  can be found, and from them
%   all the other constants A`,C`,D`,E` can be found.
%
%   A` = A*c^2 - B*c*s + C*s^2                  D` = D*c-E*s
%   B` = 2*A*c*s +(c^2-s^2)*B -2*C*c*s = 0      E` = D*s+E*c
%   C` = A*s^2 + B*c*s + C*c^2
%
% Next, we want the representation of the non-tilted ellipse to be as:
%
%       Ellipse = ( (X-X0)/a )^2 + ( (Y-Y0)/b )^2 = 1
%
%       where:  (X0,Y0) is the center of the ellipse
%               a,b     are the ellipse "radiuses" (or sub-axis)
%
% Using a square completion method we will define:
%
%       F`` = -F` + (D`^2)/(4*A`) + (E`^2)/(4*C`)
%
%       Such that:    a`*(X-X0)^2 = A`(X^2 + X*D`/A` + (D`/(2*A`))^2 )
%                     c`*(Y-Y0)^2 = C`(Y^2 + Y*E`/C` + (E`/(2*C`))^2 )
%
%       which yields the transformations:
%
%           X0  =   -D`/(2*A`)
%           Y0  =   -E`/(2*C`)
%           a   =   sqrt( abs( F``/A` ) )
%           b   =   sqrt( abs( F``/C` ) )
%
% And finally we can define the remaining parameters:
%
%   long_axis   = 2 * max( a,b )
%   short_axis  = 2 * min( a,b )
%   Orientation = phi
%
%

% initialize
orientation_tolerance = 1e-3;

% empty warning stack
warning( '' );

% prepare vectors, must be column vectors
x = x(:);
y = y(:);

% remove bias of the ellipse - to make matrix inversion more accurate. (will be added later on).
mean_x = mean(x);
mean_y = mean(y);
x = x-mean_x;
y = y-mean_y;

% the estimation for the conic equation of the ellipse
X = [x.^2, x.*y, y.^2, x, y ];
a = sum(X)/(X'*X);

% check for warnings
if ~isempty( lastwarn )
    disp( 'stopped because of a warning regarding matrix inversion' );
    ellipse_t = [];
    return
end

% extract parameters from the conic equation
[a,b,c,d,e] = deal( a(1),a(2),a(3),a(4),a(5) );

% remove the orientation from the ellipse
if ( min(abs(b/a),abs(b/c)) > orientation_tolerance )

    orientation_rad = 1/2 * atan( b/(c-a) );
    cos_phi = cos( orientation_rad );
    sin_phi = sin( orientation_rad );
    [a,b,c,d,e] = deal(...
        a*cos_phi^2 - b*cos_phi*sin_phi + c*sin_phi^2,...
        0,...
        a*sin_phi^2 + b*cos_phi*sin_phi + c*cos_phi^2,...
        d*cos_phi - e*sin_phi,...
        d*sin_phi + e*cos_phi );
    [mean_x,mean_y] = deal( ...
        cos_phi*mean_x - sin_phi*mean_y,...
        sin_phi*mean_x + cos_phi*mean_y );
else
    orientation_rad = 0;
    cos_phi = cos( orientation_rad );
    sin_phi = sin( orientation_rad );
end

% check if conic equation represents an ellipse
test = a*c;
switch (1)
case (test>0),  status = '';
case (test==0), status = 'Parabola found';  warning( 'fit_ellipse: Did not locate an ellipse' );
case (test<0),  status = 'Hyperbola found'; warning( 'fit_ellipse: Did not locate an ellipse' );
end

% if we found an ellipse return it's data
if (test>0)

    % make sure coefficients are positive as required
    if (a<0), [a,c,d,e] = deal( -a,-c,-d,-e ); end

    % final ellipse parameters
    X0          = mean_x - d/2/a;
    Y0          = mean_y - e/2/c;
    F           = 1 + (d^2)/(4*a) + (e^2)/(4*c);
    [a,b]       = deal( sqrt( F/a ),sqrt( F/c ) );
    long_axis   = 2*max(a,b);
    short_axis  = 2*min(a,b);

    % rotate the axes backwards to find the center point of the original TILTED ellipse
    R           = [ cos_phi sin_phi; -sin_phi cos_phi ];
    P_in        = R * [X0;Y0];
    X0_in       = P_in(1);
    Y0_in       = P_in(2);

    % pack ellipse into a structure
    ellipse_t = struct( ...
        'a',a,...
        'b',b,...
        'phi',orientation_rad,...
        'X0',X0,...
        'Y0',Y0,...
        'X0_in',X0_in,...
        'Y0_in',Y0_in,...
        'long_axis',long_axis,...
        'short_axis',short_axis,...
        'status','' );
else
    % report an empty structure
    ellipse_t = struct( ...
        'a',[],...
        'b',[],...
        'phi',[],...
        'X0',[],...
        'Y0',[],...
        'X0_in',[],...
        'Y0_in',[],...
        'long_axis',[],...
        'short_axis',[],...
        'status',status );
end

% check if we need to plot an ellipse with it's axes.
if (nargin>2) & ~isempty( axis_handle ) & (test>0)

    % rotation matrix to rotate the axes with respect to an angle phi
    R = [ cos_phi sin_phi; -sin_phi cos_phi ];

    % the axes
    ver_line        = [ [X0 X0]; Y0+b*[-1 1] ];
    horz_line       = [ X0+a*[-1 1]; [Y0 Y0] ];
    new_ver_line    = R*ver_line;
    new_horz_line   = R*horz_line;

    % the ellipse
    theta_r         = linspace(0,2*pi);
    ellipse_x_r     = X0 + a*cos( theta_r );
    ellipse_y_r     = Y0 + b*sin( theta_r );
    rotated_ellipse = R * [ellipse_x_r;ellipse_y_r];

    % draw
    hold_state = get( axis_handle,'NextPlot' );
    set( axis_handle,'NextPlot','add' );
    plot( new_ver_line(1,:),new_ver_line(2,:),'r' );
    plot( new_horz_line(1,:),new_horz_line(2,:),'r' );
    plot( rotated_ellipse(1,:),rotated_ellipse(2,:),'r' );
    set( axis_handle,'NextPlot',hold_state );
end


function [m_database V_PCA V_Fisher ProjectedImages_Fisher] = FisherfaceCore(T)
% Use Principle Component Analysis (PCA) and Fisher Linear Discriminant (FLD) to determine the most
% discriminating features between images of faces.
%
% Description: This function gets a 2D matrix, containing all training image vectors
% and returns 4 outputs which are extracted from training database.
% Suppose Ti is a training image, which has been reshaped into a 1D vector.
% Also, P is the total number of MxN training images and C is the number of
% classes. At first, centered Ti is mapped onto a (P-C) linear subspace by V_PCA
% transfer matrix: Zi = V_PCA * (Ti - m_database).
% Then, Zi is converted to Yi by projecting onto a (C-1) linear subspace, so that
% images of the same class (or person) move closer together and images of difference
% classes move further apart: Yi = V_Fisher' * Zi = V_Fisher' * V_PCA' * (Ti - m_database)
%
% Argument:      T                      - (M*NxP) A 2D matrix, containing all 1D image vectors.
%                                         All of 1D column vectors have the same length of M*N
%                                         and 'T' will be a MNxP 2D matrix.
%
% Returns:       m_database             - (M*Nx1) Mean of the training database
%                V_PCA                  - (M*Nx(P-C)) Eigen vectors of the covariance matrix of the
%                                         training database
%                V_Fisher               - ((P-C)x(C-1)) Largest (C-1) eigen vectors of matrix J = inv(Sw) * Sb
%                ProjectedImages_Fisher - ((C-1)xP) Training images, which are projected onto Fisher linear space
%
% See also: EIG

% Original version by Amir Hossein Omidvarnia, October 2007
%                     Email: aomidvar@ece.ut.ac.ir

Class_number = ( size(T,2) )/2; % Number of classes (or persons)
Class_population = 2; % Number of images in each class
P = Class_population * Class_number; % Total number of training images

%%%%%%%%%%%%%%%%%%%%%%%% calculating the mean image
m_database = mean(T,2);

%%%%%%%%%%%%%%%%%%%%%%%% Calculating the deviation of each image from mean image
A = T - repmat(m_database,1,P);

%%%%%%%%%%%%%%%%%%%%%%%% Snapshot method of Eigenface algorithm
L = A'*A; % L is the surrogate of covariance matrix C=A*A'.
[V D] = eig(L); % Diagonal elements of D are the eigenvalues for both L=A'*A and C=A*A'.

%%%%%%%%%%%%%%%%%%%%%%%% Sorting and eliminating small eigenvalues
L_eig_vec = [];
for i = 1 : P-Class_number
    L_eig_vec = [L_eig_vec V(:,i)];
end

%%%%%%%%%%%%%%%%%%%%%%%% Calculating the eigenvectors of covariance matrix 'C'
V_PCA = A * L_eig_vec; % A: centered image vectors

%%%%%%%%%%%%%%%%%%%%%%%% Projecting centered image vectors onto eigenspace
% Zi = V_PCA' * (Ti-m_database)
ProjectedImages_PCA = [];
for i = 1 : P
    temp = V_PCA'*A(:,i);
    ProjectedImages_PCA = [ProjectedImages_PCA temp];
end

%%%%%%%%%%%%%%%%%%%%%%%% Calculating the mean of each class in eigenspace
m_PCA = mean(ProjectedImages_PCA,2); % Total mean in eigenspace
m = zeros(P-Class_number,Class_number);
Sw = zeros(P-Class_number,P-Class_number); % Initialization os Within Scatter Matrix
Sb = zeros(P-Class_number,P-Class_number); % Initialization of Between Scatter Matrix

for i = 1 : Class_number
    m(:,i) = mean( ( ProjectedImages_PCA(:,((i-1)*Class_population+1):i*Class_population) ), 2 )';

    S  = zeros(P-Class_number,P-Class_number);
    for j = ( (i-1)*Class_population+1 ) : ( i*Class_population )
        S = S + (ProjectedImages_PCA(:,j)-m(:,i))*(ProjectedImages_PCA(:,j)-m(:,i))';
    end

    Sw = Sw + S; % Within Scatter Matrix
    Sb = Sb + (m(:,i)-m_PCA) * (m(:,i)-m_PCA)'; % Between Scatter Matrix
end

%%%%%%%%%%%%%%%%%%%%%%%% Calculating Fisher discriminant basis's
% We want to maximise the Between Scatter Matrix, while minimising the
% Within Scatter Matrix. Thus, a cost function J is defined, so that this condition is satisfied.
[J_eig_vec, J_eig_val] = eig(Sb,Sw); % Cost function J = inv(Sw) * Sb
J_eig_vec = fliplr(J_eig_vec);

%%%%%%%%%%%%%%%%%%%%%%%% Eliminating zero eigens and sorting in descend order
for i = 1 : Class_number-1
    V_Fisher(:,i) = J_eig_vec(:,i); % Largest (C-1) eigen vectors of matrix J
end

%%%%%%%%%%%%%%%%%%%%%%%% Projecting images onto Fisher linear space
% Yi = V_Fisher' * V_PCA' * (Ti - m_database)
for i = 1 : Class_number*Class_population
    ProjectedImages_Fisher(:,i) = V_Fisher' * ProjectedImages_PCA(:,i);
end


function [out,im] = pyramid (infile,level,outfile)
% PYRAMID (infile,singvals,outfile)
% Image compression based on Gaussian Pyramid.
%  infile is input file name present in the current directory
%  level is a positive integer (the scaling factor)
%  outfile is output file name which will be created.
%
% It is possble to change the frequency response of the low pass
% gaussian filter changing the parameter "varianza" (see below).
%
% Example of use:
% [out,im]=pyramid('input_image.bmp',3,'output_image.bmp');
% im will be a double cell array with the 3 resampled layers
% and out will be the reconstructed image.
%
% The input image A can be RGB or GRAYSCALE.
%
% RGB case:
% If A is of class double, all values must be in the range [0,1],
% and A must be m-by-n-by-3.
% If A is of class uint16 or uint8, A must be m-by-n-by-3.
%
% GRAYSCALE case:
% If A is of class double, all values must be in the range [0,1],
% and the number of dimensions of A must be 2. If A is of class
% uint16 or uint8, the number of dimensions of A must be 2.
% uint16 or double.
%
%
% References:
%
% For more details concernings the algorithm implemented please visit the following links:
%
%  http://www.cs.ucf.edu/courses/cap6411/lect826h.PDF
%
%  http://www.wisdom.weizmann.ac.il/~maksimf/ex2/g3.html
%
%
% Please contribute if you find this software useful.
% Report bugs to luigi.rosa@tiscali.it
%
%
%*****************************************************************
% Luigi Rosa
% Via Centrale 27
% 67042 Civita di Bagno
% L'Aquila --- ITALY
% email  luigi.rosa@tiscali.it
% mobile +39 340 3463208
% http://utenti.lycos.it/matlab
%*****************************************************************
%
%
if (exist(infile)==2)
   a = imread(infile);
   figure('Name','Input image');
   imshow(a);
else
   warndlg('The file does not exist.',' Warning ');
   im=[];
   out=[];
   return
end

%------------------------------------
% The low-pass filter ---------------
%------------------------------------
% Gaussian Low Pass Filter ----------
%------------------------------------
x=[-16:1:16];
y=[-16:1:16];
dimx=size(x,2);
dimy=size(y,2);
varianza=2.3;
filtro=zeros(dimx,dimy);
for ii=1:dimx
    for jj=1:dimy
        esponente=exp(-(x(ii)^2+y(jj)^2)/(varianza));
        filtro(ii,jj)=esponente;
    end
end
% normalization
filtro=filtro/sum(sum(filtro));
%------------------------------------
%------------------------------------

if isgray(a)

    dvalue = double(a);
    dx     = size(dvalue,1);
    dy     = size(dvalue,2);
    dmin   = min(dx,dy);

    red_p  = floor(log2(dmin));
    red    = min(red_p,level);

    im=cell(red+1,1);

    % Image compression
    im{1}=dvalue;
    for ii=1:red
         % low-pass filter the image
         filtered        = conv2fft(dvalue,filtro,'same');
         % downsampling the image
         sottocampionato = dyaddown(filtered,1,'m');
         % save image
         im{ii+1}        = sottocampionato;
         % next step
         dvalue          = sottocampionato;
    end

    % Image reconstruction
    for ii=1:red
         % upsampling the image
         sovracampionato = dyadup(dvalue,0,'m');
         % low-pass filter the image (there is a scaling factor)
         filtered        = 4*conv2fft(sovracampionato,filtro,'same');
         % next step
         dvalue          = filtered;
    end

    if isa(a,'uint8')
        for ii=2:red+1
             testo=strcat('Image at level-',num2str(ii-1));
             figure('Name',testo);
             imshow(uint8(im{ii}));
             pause(0.6);
        end
        figure('Name','Reconstrunction');
        imshow(uint8(dvalue));
        pause(0.6);
        out=uint8(dvalue);

    end

    if isa(a,'uint16')
        for ii=2:red+1
             testo=strcat('Image at level-',num2str(ii-1));
             figure('Name',testo);
             imshow(uint16(im{ii}));
             pause(0.6);
        end
        figure('Name','Reconstrunction');
        imshow(uint16(dvalue));
        pause(0.6);
        out=uint16(dvalue);

    end

    if isa(a,'double')
        for ii=2:red+1
             testo=strcat('Image at level-',num2str(ii-1));
             figure('Name',testo);
             imshow(uint32(im{ii}));
             pause(0.6);
        end
        figure('Name','Reconstrunction');
        imshow((dvalue));
        pause(0.6);
        out=uint32(dvalue);

   end

    imwrite(out, outfile);
    return;
end

%------------------------------------------------------
if isrgb(a)
    dvalue = double(a);
    dx     = size(dvalue,1);
    dy     = size(dvalue,2);
    dmin   = min(dx,dy);

    red_p  = floor(log2(dmin));
    red    = min(red_p,level);

    im=cell(red+1,1);

    % Image compression
    im{1}=dvalue;
    for ii=1:red
         % low-pass filter the image
         filtered_r        = conv2fft(dvalue(:,:,1),filtro,'same');
         filtered_g        = conv2fft(dvalue(:,:,2),filtro,'same');
         filtered_b        = conv2fft(dvalue(:,:,3),filtro,'same');
         % downsampling the image
         sottocampionato_r = dyaddown(filtered_r,1,'m');
         sottocampionato_g = dyaddown(filtered_g,1,'m');
         sottocampionato_b = dyaddown(filtered_b,1,'m');
         % save image
         clear sottocampionato;
         sottocampionato(:,:,1)=sottocampionato_r;
         sottocampionato(:,:,2)=sottocampionato_g;
         sottocampionato(:,:,3)=sottocampionato_b;
         im{ii+1}        = sottocampionato;
         % next step
         dvalue          = sottocampionato;
    end

    % Image reconstruction
    for ii=1:red
         % upsampling the image
         sovracampionato_r = dyadup(dvalue(:,:,1),0,'m');
         sovracampionato_g = dyadup(dvalue(:,:,2),0,'m');
         sovracampionato_b = dyadup(dvalue(:,:,3),0,'m');
         % low-pass filter the image (there is a scaling factor)
         filtered_r        = 4*conv2fft(sovracampionato_r,filtro,'same');
         filtered_g        = 4*conv2fft(sovracampionato_g,filtro,'same');
         filtered_b        = 4*conv2fft(sovracampionato_b,filtro,'same');
         % next step
         clear dvalue;
         dvalue(:,:,1)          = filtered_r;
         dvalue(:,:,2)          = filtered_g;
         dvalue(:,:,3)          = filtered_b;
    end

    if isa(a,'uint8')
        for ii=2:red+1
             testo=strcat('Image at level-',num2str(ii-1));
             figure('Name',testo);
             imshow(uint8(im{ii}));
             pause(0.6);
        end
        figure('Name','Reconstrunction');
        imshow(uint8(dvalue));
        pause(0.6);
        out=uint8(dvalue);

    end

    if isa(a,'uint16')
        for ii=2:red+1
             testo=strcat('Image at level-',num2str(ii-1));
             figure('Name',testo);
             imshow(uint16(im{ii}));
             pause(0.6);
        end
        figure('Name','Reconstrunction');
        imshow(uint16(dvalue));
        pause(0.6);
        out=uint16(dvalue);

    end

    if isa(a,'double')
        for ii=2:red+1
             testo=strcat('Image at level-',num2str(ii-1));
             figure('Name',testo);
             imshow((im{ii}));
             pause(0.6);
        end
        figure('Name','Reconstrunction');
        imshow((dvalue));
        pause(0.6);
        out=(dvalue);

   end

    imwrite(out, outfile);
    return;

end


function [model] = svRegression(X,y, epsilon)
% Compute sizes
[n,d] = size(X);

% Add bias variable
Z = [ones(n,1) X];

one = zeros((d+1) + n, 1);
one(d+2:d+1+n) = 1;

A = [Z -eye(n, n);
    -Z -eye(n, n);
    zeros(n,d+1) -eye(n,n)];

b = [y+epsilon; -y+epsilon; zeros(n,1)];

x = linprog(one, A, b);
w = x(1:d+1);

model.w = w;
model.predict = @predict;

end

function [yhat] = predict(model,Xhat)
[t,d] = size(Xhat);
Zhat = [ones(t,1) Xhat];
yhat = Zhat*model.w;
end


function MISE=GaussKernelGaussMixMISE(n,H,w,mu,P)
%%GAUSSKERNELGAUSSMIXMISE Given a Gaussian mixture probability density
%            function (PDF) that one is attempting to approximate using a
%            kernel density estimator with Gaussian kernels all having
%            square root covariance matrix H, determine the mean integrated
%            squared error (MISE) of the estimate. The MISE is the expected
%            value of int_{R}(\hat{f}(x)-f(x))^2dx where \hat{f}(x) is the
%            kernel estimator based on a set of random samples of the true
%            density f(x) (since it is based on random samples, the mean
%            part is the expected value being taken over all possible
%            samples). This function is useful for assessing different
%            kernel bandwidth estimators (estimators of H) when using
%            Gaussian mixture sample problems.
%
%INPUTS: n The number of samples that the kernel-density estimator will
%          use.
%        H The dXd bandwidth matrix used in the kernel density estimator
%          (as one would use in the function kernelApprox).
%        w A numCompX1 or 1XnumComp vector of the weights associated with
%          each of the numComp components of the Gaussian mixture.
%       mu The dXnumComp matrix of the mean of each component of the
%          Gaussian mixture.
%        P The dXdXnumComp set of covariance matrices of the components of
%          the Gaussian mixture.
%
%OUTPUTS: MISE The MISE of thekernel estimator with the given number of
%              samples and bandwidth matrix.
%
%This function implements the formula for the MISE in an arbitrary number
%of dimensions in Section 4 of [1].
%
%REFERENCES:
%[1] M. P. Wand and M. C. Jones, "Comparison of smoothing parameterizations
%    in bivariate kernel density estimation," Journal of the American
%    Statistical Association, vol. 88, no. 422, pp. 520-528, Jun. 1993.
%
%December 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The number of components in the
k=size(mu,2);
d=size(mu,1);

%The definition of the bandwidth used in the paper is the square of what is
%used here.
H=H*H';

Omega0=zeros(k,k);
Omega1=zeros(k,k);
Omega2=zeros(k,k);

for l=1:k
    for lp=l:k
       Sigma=P(:,:,l)+P(:,:,lp);

       Omega0(l,lp)=GaussianD.PDF(mu(:,l),mu(:,lp),Sigma);
       Omega1(l,lp)=GaussianD.PDF(mu(:,l),mu(:,lp),Sigma+H);
       Omega2(l,lp)=GaussianD.PDF(mu(:,l),mu(:,lp),Sigma+2*H);
    end
end

MISE=(1/n)*(4*pi)^(-d/2)*1/sqrt(det(H))+w(:)'*((1-1/n)*Omega2-2*Omega1+Omega0)*w(:);

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 = p14_exact ( dim_num )

%*****************************************************************************80
%
%% P14_EXACT returns the exact integral for problem 14.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Output, real VALUE, the exact value of the integral.
%
  value = ( - 1.0 / 3.0 ) * ( 1.0 - ( -1.0 / 2.0 )^dim_num );

  return
end


function [Q,err] = quadgr(fun,a,b,tol,trace,varargin)
%QUADGR Gauss-Legendre quadrature with Richardson extrapolation.
%   [Q,ERR] = QUADGR(FUN,A,B,TOL) approximates the integral of a function
%   FUN from A to B with an absolute error tolerance TOL. FUN is a function
%   handle and must accept vector arguments. TOL is 1e-6 by default. Q is
%   the integral approximation and ERR is an estimate of the absolute error.
%
%   QUADGR uses a 12-point Gauss-Legendre quadrature. The error estimate is
%   based on successive interval bisection. Richardson extrapolation
%   accelerates the convergence for some integrals, especially integrals
%   with endpoint singularities.
%
%   QUADGR(FUN,A,B,TOL,TRACE) with non-zero TRACE displays the
%   extrapolation table.
%
%   QUADGR can also be used as the quadrature in DBLQUAD and TRIPLEQUAD.
%
%   Examples:
%     Q = quadgr(@(x) log(x),0,1)
%     [Q,err] = quadgr(@(x) exp(x),0,9999i*pi)
%     [Q,err] = quadgr(@(x) sqrt(4-x.^2),0,2,1e-12)
%     [Q,err] = quadgr(@(x) x.^-0.75,0,1)
%     [Q,err] = quadgr(@(x) 1./sqrt(1-x.^2),-1,1)
%     [Q,err] = quadgr(@(x) exp(-x.^2),-inf,inf,1e-9) % sqrt(pi)
%     [Q,err] = quadgr(@(x) cos(x).*exp(-x),0,inf,1e-9)
%
%   See also QUAD, QUADGK, DBLQUAD, TRIPLEQUAD

%   Author: Jonas Lundgren <splinefit@gmail.com> 2009

%   2009-03-17  First published
%   2010-04-14  Adapted to DBLQUAD and TRIPLEQUAD (varargin added)

if nargin < 3, help quadgr, return, end
if nargin < 4 || isempty(tol), tol = 1.e-6; end;
if nargin < 5 || isempty(trace), trace = 0; end;

% Order limits (required if infinite limits)
if a == b
    Q = b - a;
    err = b - a;
    return
elseif a > b
    reverse = true;
    atmp = a;
    a = b;
    b = atmp;
else
    reverse = false;
end

% Infinite limits
if isinf(a) || isinf(b)
    % Check real limits
    if ~isreal(a) || ~isreal(b) || isnan(a) || isnan(b)
        error('quadgr:inflim','Infinite intervals must be real.')
    end
    % Change of variable
    if isfinite(a) && isinf(b)
        % a to inf
        fun1 = @(t,varargin) fun(a + t./(1-t), varargin{:})./(1-t).^2;
        [Q,err] = quadgr(fun1,0,1,tol,trace,varargin{:});
    elseif isinf(a) && isfinite(b)
        % -inf to b
        fun2 = @(t,varargin) fun(b + t./(1+t), varargin{:})./(1+t).^2;
        [Q,err] = quadgr(fun2,-1,0,tol,trace,varargin{:});
    else % -inf to inf
        fun1 = @(t,varargin) fun(t./(1-t), varargin{:})./(1-t).^2;
        fun2 = @(t,varargin) fun(t./(1+t), varargin{:})./(1+t).^2;
        [Q1,err1] = quadgr(fun1,0,1,tol/2,trace,varargin{:});
        [Q2,err2] = quadgr(fun2,-1,0,tol/2,trace,varargin{:});
        Q = Q1 + Q2;
        err = err1 + err2;
    end
    % Reverse direction
    if reverse
        Q = -Q;
    end
    return
end

% Gauss-Legendre quadrature (12-point)
xq = [0.12523340851146894; 0.36783149899818018; 0.58731795428661748; ...
      0.76990267419430469; 0.9041172563704748; 0.98156063424671924];
wq = [0.24914704581340288, 0.23349253653835478, 0.20316742672306584, ...
      0.16007832854334636, 0.10693932599531818, 0.047175336386511842];
xq = [xq; -xq];
wq = [wq, wq];
nq = length(xq);

% Initiate vectors
maxit = 17;                 % Max number of iterations
Q0 = zeros(maxit,1);       	% Quadrature
Q1 = zeros(maxit,1);       	% First Richardson extrapolation
Q2 = zeros(maxit,1);       	% Second Richardson extrapolation

% One interval
hh = (b - a)/2;             % Half interval length
x = (a + b)/2 + hh*xq;      % Nodes
% Quadrature
Q0(1) = hh*wq*fun(x,varargin{:});

% Successive bisection of intervals
for k = 2:maxit

    % Interval bisection
    hh = hh/2;
    x = [x + a; x + b]/2;
    % Quadrature
    Q0(k) = hh*wq*sum(reshape(fun(x,varargin{:}),nq,[]),2);

    % Richardson extrapolation
    if k >= 5
        Q1(k) = richardson(Q0,k);
        Q2(k) = richardson(Q1,k);
    elseif k >= 3
        Q1(k) = richardson(Q0,k);
    end

    % Estimate absolute error
    if k >= 6
        Qv = [Q0(k), Q1(k), Q2(k)];
        Qw = [Q0(k-1), Q1(k-1), Q2(k-1)];
    elseif k >= 4
        Qv = [Q0(k), Q1(k)];
        Qw = [Q0(k-1), Q1(k-1)];
    else
        Qv = Q0(k);
        Qw = Q0(k-1);
    end
    [err,j] = min(abs(Qv - Qw));
    Q = Qv(j);

    % Convergence
    if err < tol || ~isfinite(Q)
        break;
    end

end

% Convergence check
if ~isfinite(Q)
    warning('quadgr:infnan','Integral approximation is Infinite or NaN.')
elseif err > tol
    warning('quadgr:maxiter','Max number of iterations reached without convergence.')
end

% The error estimate should not be zero
err = err + 2*eps(Q);
% Reverse direction
if reverse
	Q = -Q;
end

% Display convergence
if trace
    disp(' ')
    disp([Q0(1:k) Q1(1:k) Q2(1:k)])
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function R = richardson(Q,k)
% Richardson extrapolation with parameter estimation
if Q(k) ~= Q(k-1)
    c = real((Q(k-1)-Q(k-2))/(Q(k)-Q(k-1))) - 1;
else
    c = 1;
end
% The lower bound 0.07 admits the singularity x.^-0.9
c = max(c,0.07);
R = Q(k) + (Q(k) - Q(k-1))/c;


function [ lambda, v, it_num ] = power_method2 ( n, a, x_init, it_max, tol )

%*****************************************************************************80
%
%% POWER_METHOD2 applies the power method for possibly complex eigenvalues.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 July 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Eric VanDeVelde,
%    Concurrent Scientific Programming,
%    Springer, 1994,
%    ISBN: 0-387-94195-9,
%    LC: QA76.58.V35.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(N,N), the matrix.
%
%    Input, real X(N), the initial estimate for the eigenvector.
%
%    Input, integer IT_MAX, the maximum number of iterations to take.
%    1 <= IT_MAX.
%
%    Input, real TOL, an error tolerance.
%
%    Output, complex LAMBDA, the estimate for the eigenvalue.
%
%    Output, complex V(N), the estimate for the eigenvector.
%
%    Output, integer IT_NUM, the number of iterations taken.
%
  it_num = 0;
%
%  Compute data necessary to start the iteration.
%
  x(1:n,1) = x_init(1:n);
  pi_xx = x(1:n)' * x(1:n);
  x(1:n) = x(1:n) / pi_xx;
  y(1:n,1) = a(1:n,1:n) * x(1:n);
  pi_xy = x(1:n)' * y(1:n);
  pi_yy = y(1:n)' * y(1:n);

  for it = 1 : it_max

    if ( pi_yy - pi_xy * pi_xy < tol * tol * pi_yy )
      lambda = pi_xy;
      v(1:n) = y(1:n) / sqrt ( pi_yy );
      return
    end

    z(1:n,1) = a(1:n,1:n) * y(1:n);

    pi_xz = x(1:n)' * z(1:n);
    pi_yz = y(1:n)' * z(1:n);
    pi_zz = z(1:n)' * z(1:n);

    alpha = - ( pi_yz - pi_xy * pi_xz ) / ( pi_yy - pi_xy * pi_xy );
    beta = ( pi_xy * pi_yz - pi_yy * pi_xz ) / ( pi_yy - pi_xy * pi_xy );
    gamma = pi_zz + alpha * alpha * pi_yy + beta * beta ...
      + 2.0 * ( alpha * pi_yz + beta * pi_xz + alpha * beta * pi_xy );

    if ( gamma < tol * tol * pi_zz & alpha * alpha < 4.0 * beta )

      lambda_real = - alpha / 2.0;
      lambda_imag = sqrt ( 4.0D+00 * beta - alpha * alpha ) / 2.0;
      lambda = lambda_real + lambda_imag * sqrt ( - 1.0 );

      v(1:n) = ( lambda * y(1:n) - z(1:n) ) ...
       / sqrt ( beta * pi_yy + alpha * pi_yz + pi_zz );

      return
    end

    x(1:n) = y(1:n) / sqrt ( pi_yy );
    y(1:n) = z(1:n) / sqrt ( pi_yy );

    pi_xy = pi_yz / pi_yy;
    pi_yy = pi_zz / pi_yy;

    it_num = it;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POWER_METHOD2 - Fatal error!\n' );
  fprintf ( 1, '  Convergence was not reached.\n' );

  error ( 'POWER_METHOD2 - Fatal error!' );
end


% 埃特金方法加速迭代方法求解方程
clear;
format long;
tol = 1e-10;
N = 100;
x0 = 0.5;
phi = @(x) exp(-x);
%phi = @(x) (x+1)^(1/3);

for k = 1 : N
    x1 = phi(x0);
    x2 = phi(x1);
    x2 = x2 - (x2 - x1)^2 / (x2 - 2 * x1 + x0);
    if abs(x2 - x0) < tol
        fprintf('方程的正根: %10.8f\n', x1);
        break;
    end
    x0 = x2;
end
if k == N
    fprintf('迭代方法失败\n');
end
fprintf('迭代次数: %d\n', k);


function h = huber_pot(t, d)
%| function h = huber_pot(t, d)
%| huber potential function

if nargin < 1, ir_usage, end
if nargin < 2, huber_pot_test, return, end

h = t.^2 / 2;
ii = abs(t) > d;
h(ii) = d * abs(t(ii)) - d.^2/2;

function huber_pot_test
t = linspace(-9,9,101);
delta = 4;
plot(t, huber_pot(t, delta), '-', delta, huber_pot(delta, delta), 'o')


function mean = extreme_values_mean ( a, b )

%*****************************************************************************80
%
%% EXTREME_VALUES_MEAN returns the mean of the Extreme Values PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < B.
%
%    Output, real MEAN, the mean of the PDF.
%
  mean = a + b * euler_constant ( );

  return
end


function tinv = t_inv (x, v, mu, sigma)
% T_INV   Inverse of Student's T cumulative distribution function (cdf).
%    X=TINV(P,V,MU,SIGMA) returns the inverse of Student's T cdf with V degrees
%    of freedom, at the values in P, with mean MU and standard deviation
%    SIGMA.
%
%    The size of X is the common size of P and V. A scalar input
%    functions as a constant matrix of the same size as the other input.

% Copyright (c) 1995-1997, 2005-2007 Kurt Hornik

% 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 < 4
  sigma = 1;
end
if nargin < 3
  mu = 0;
end
if (~isscalar(mu) || ~isscalar(sigma))
  if ~((size(x,1) == size(mu,1)) && (size(mu,1) == size(sigma,1)))
    error ('norm_inv: x, mu and sigma must be of common size or scalars');
  end
end

x = (x-mu)./sigma;

if (nargin < 2)
  error('must give atleast 2 input arguments')
end

if (~isscalar(v))

  if (size(x,1) ~= size(v,1))
    error ('tinv: x and v must be of common size or scalar');
  end
end

tinv = zeros(size(x));

k = find( (x<0) || (x > 1) || isnan(x) || (v<0));
if (any(k))
  tinv(k) = NaN;
end

k = find ((x == 0) & (v > 0));
if (any (k))
  tinv(k) = -Inf;
end

k = find ((x == 1) & (v > 0));
if (any (k))
  tinv(k) = Inf;
end

k = find ((x > 0) & (x < 1) & (v > 0) & (v < 10000));
if (any (k))
  if (isscalar (v))
    tinv(k) = (sign (x(k) - 1/2) ...
    .* sqrt (v .* (1 ./ beta_inv (2*min(x(k),1-x(k)), ...
    v/2,1/2)-1)));
  else
    tinv(k) = (sign(x(k)-1/2) ...
    .* sqrt (v(k).*(1./beta_inv(2*min(x(k), 1-x(k)), ...
    v(k)/2,1/2)-1)));
  end
end

% For large v, use the quantiles of the standard normal
k = find ((x > 0) & (x < 1) & (v >= 10000));
if (any (k))
  tinv(k) = sqrt (2)*erfinv(2*x(k)-1);
end

end

% inv = sqrt (2) * erfinv (2 * x - 1);


clf
hold on;
x = 0:0.01:8*pi;
data = [x', sin(x)'];
plot(data(:,1), data(:,2));
axis([0, 8*pi, -3, 3]);
legend('original data');
grid on;

pause;

clf
hold on;

data_length = length(data)
for idx = 1500:2000
    data(idx,2) = cos(data(idx,1));
end;
plot(data(:,1), data(:,2));
axis([0, 8*pi, -3, 3]);
legend('human modified data');
grid on;

pause;

%in this demo, we only use best_so_far_loc while we plot the figure
n = 500;
[best_so_far_dist, best_so_far_loc] = HOTSAX(data(:,2), n, 'nseg 4')

%plot the figure
hold on;
if ~isnan(best_so_far_loc)
    plot_x = data(best_so_far_loc:best_so_far_loc+n, 1);
    plot_y = data(best_so_far_loc:best_so_far_loc+n, 2);
    plot(plot_x,plot_y, 'r');
    legend('human modified data', 'anomaly detect');
end
hold off;


function [px, py, gx, gy] = position_3_link(z,P)
%[px,py,gx,gy] = POSITION_3_LINK(Z,P)
%
%FUNCTION:  This function computes the cartesian positions
%    of the CoM and tip of each link
%INPUTS:
%
%
%OUTPUTS:
%    px = [nLink X nTime] position of the end of each link
%    py = [nLink X nTime] position of the end of each link
%    gx = [nLink X nTime] position of the CoM of each link
%    gy = [nLink X nTime] position of the CoM of each link
%
%NOTES:
%    This file was automatically generated by writePosition.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint

th1 = z(1,:);
th2 = z(2,:);
th3 = z(3,:);

nTime = length(th1);
px = zeros(3,nTime);
py = zeros(3,nTime);
gx = zeros(3,nTime);
gy = zeros(3,nTime);

px(1,:) = l1.*cos(th1);
py(1,:) = l1.*sin(th1);
gx(1,:) = d1.*cos(th1);
gy(1,:) = d1.*sin(th1);

px(2,:) = l1.*cos(th1) + l2.*cos(th2);
py(2,:) = l1.*sin(th1) + l2.*sin(th2);
gx(2,:) = d2.*cos(th2) + l1.*cos(th1);
gy(2,:) = d2.*sin(th2) + l1.*sin(th1);

px(3,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
py(3,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);
gx(3,:) = d3.*cos(th3) + l1.*cos(th1) + l2.*cos(th2);
gy(3,:) = d3.*sin(th3) + l1.*sin(th1) + l2.*sin(th2);

end


function sigmaEq = equivalentStress(sigma,varargin)
% von Mises equivalent stress
%
% Input
%  sigma - @stressTensor
%
% Output
%  s - double
%

s = sigma.deviatoricStress;
sigmaEq = sqrt(3/2 * s:s);

% the following code gives the same result
% is more efficient but less readable

% M = sigma.M;
% sigmaEq = sqrt(M(1,1,:).^2 + M(2,2,:).^2 + M(3,3,:).^2 ...
%   - M(1,1,:).*M(2,2,:) - M(2,2,:).*M(3,3,:)- M(1,1,:).*M(3,3,:) ...
%   + 3*(M(1,2).^2 + M(1,3).^2 + M(2,3).^2 ));


function exact = p44_exact ( )

%*****************************************************************************80
%
%% P44_EXACT returns the exact integral for problem 44.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the value of the integral.
%
  alpha = p44_param_get ( );

  exact = ( 20.0 * sin ( 2.0^alpha ) ...
    - 2.0^alpha * cos ( 2.0^alpha ) ...
    + 2.0^alpha * exp ( -20.0 ) ) / ( 400.0 + 4.0^alpha );

  return
end


function [g,A] = minusOne_v2(f)
%minusOne Multiplies an input array by (-1)^x+y.
%   [G,A] = minusOne(F) multiplies F by (-1)^x+y to produce G. F can be
%   a 1-D or 2-D array. On the output, G = (-1)^(x+y).*F, and A is the
%   array of values (-1)^(x+y). Output G is floating point with values
%   in the same numerical range as F.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Preliminaries.

% Convert input to floating point for multiplication later.
f = im2double(f);

% Image size.
[M,N] = size(f);

x = (0:M - 1)';
y = 0:N - 1;

A = (-1).^(x + y);

% Multiply input by matrix.
g = A.*f;


function geometry_test058 ( )

%*****************************************************************************80
%
%% TEST058 tests PLANE_IMP_POINT_DIST_3D, PLANE_IMP_POINT_DIST_SIGNED_3D;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2009
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;
  ntest = 4;
%
%  This is the plane Z = 10.
%
  a =    0.0;
  b =    0.0;
  c =    1.0;
  d = - 10.0;

  ptest(1:dim_num,1:ntest) = [ ...
    -12.0, 14.0,  0.0; ...
      7.0,  8.0,  9.0; ...
      1.0,  2.0, 10.0; ...
      0.0,  0.0, 12.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST058\n' );
  fprintf ( 1, '  PLANE_IMP_POINT_DIST_3D computes the distance\n' );
  fprintf ( 1, '    between an implicit plane and a point in 3D;\n' );
  fprintf ( 1, '  PLANE_IMP_POINT_DIST_SIGNED 3D computes the\n' );
  fprintf ( 1, '    signed distance between an implicit plane\n' );
  fprintf ( 1, '    and a point in 3D.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  For all tests, we use the implicit plane with\n' );
  fprintf ( 1, '  (A,B,C,D) = %f  %f  %f  %f\n', a, b, c, d );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  (X,Y,Z)  DISTANCE   SIGNED_DISTANCE\n' );
  fprintf ( 1, '\n' );

  for i = 1 : ntest

    p(1:dim_num) = ptest(1:dim_num,i);

    dist = plane_imp_point_dist_3d ( a, b, c, d, p );

    dist_signed = plane_imp_point_dist_signed_3d ( a, b, c, d, p );

    fprintf ( 1, '  %12f  %12f  %12f  %12f  %12f\n', p(1:dim_num), dist, dist_signed );

  end

  return
end


function [J grad] = nnCostFunction(nn_params, ...
                                   input_layer_size, ...
                                   hidden_layer_size, ...
                                   num_labels, ...
                                   X, y, lambda)
%NNCOSTFUNCTION Implements the neural network cost function for a two layer
%neural network which performs classification
%   [J grad] = NNCOSTFUNCTON(nn_params, hidden_layer_size, numn,/_labels, ...
%   X, y, lambda) computes the cost and gradient of the neural network. The
%   parameters for the neural network are "unrolled" into the vector
%   nn_params and need to be converted back into the weight matrices.
%
%   The returned parameter grad should be a "unrolled" vector of the
%   partial derivatives of the neural network.
%

% Reshape nn_params back into the parameters Theta1 and Theta2, the weight matrices
% for our 2 layer neural network
Theta1 = reshape(nn_params(1:hidden_layer_size * (input_layer_size + 1)), ...
                 hidden_layer_size, (input_layer_size + 1));

Theta2 = reshape(nn_params((1 + (hidden_layer_size * (input_layer_size + 1))):end), ...
                 num_labels, (hidden_layer_size + 1));

% Setup some useful variables
m = size(X, 1);

% You need to return the following variables correctly
J = 0;
Theta1_grad = zeros(size(Theta1));
Theta2_grad = zeros(size(Theta2));

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the code by working through the
%               following parts.
%
% Part 1: Feedforward the neural network and return the cost in the
%         variable J. After implementing Part 1, you can verify that your
%         cost function computation is correct by verifying the cost
%         computed in ex4.m
%
% Part 2: Implement the backpropagation algorithm to compute the gradients
%         Theta1_grad and Theta2_grad. You should return the partial derivatives of
%         the cost function with respect to Theta1 and Theta2 in Theta1_grad and
%         Theta2_grad, respectively. After implementing Part 2, you can check
%         that your implementation is correct by running checkNNGradients
%
%         Note: The vector y passed into the function is a vector of labels
%               containing values from 1..K. You need to map this vector into a
%               binary vector of 1's and 0's to be used with the neural network
%               cost function.
%
%         Hint: We recommend implementing backpropagation using a for-loop
%               over the training examples if you are implementing it for the
%               first time.
%
% Part 3: Implement regularization with the cost function and gradients.
%
%         Hint: You can implement this around the code for
%               backpropagation. That is, you can compute the gradients for
%               the regularization separately and then add them to Theta1_grad
%               and Theta2_grad from Part 2.
%
% FORWARD PROPOGATION
a1 = [ones(m,1) X];
z2 = (a1*Theta1');
a2 = [ones(size(z2,1),1) sigmoid(z2)];
h_theta = sigmoid(a2*Theta2');
a3 = h_theta;
y_matrix = eye(num_labels)(y,:);
J = (-sum(sum(y_matrix.*log(h_theta))) - sum(sum((1-y_matrix).*(log(1-h_theta)))))/m;

% REGULARIZATION
regularization_term = (lambda/(2*m))*((sum(sum(Theta1(:,2:end).^2))) + sum(sum(Theta2(:,2:end).^2)));
J = J + regularization_term;

% BACK PROPOGATION
d3 = a3 - y_matrix;                                             % has same dimensions as a3
d2 = (d3*Theta2).*[ones(size(z2,1),1) sigmoidGradient(z2)];     % has same dimensions as a2

D1 = d2(:,2:end)' * a1;    % has same dimensions as Theta1
D2 = d3' * a2;    % has same dimensions as Theta2

Theta1_grad = Theta1_grad + (1/m) * D1;
Theta2_grad = Theta2_grad + (1/m) * D2;

% REGULARIZATION OF THE GRADIENT

Theta1_grad(:,2:end) = Theta1_grad(:,2:end) + (lambda/m)*(Theta1(:,2:end));
Theta2_grad(:,2:end) = Theta2_grad(:,2:end) + (lambda/m)*(Theta2(:,2:end));
% -------------------------------------------------------------

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

% Unroll gradients
grad = [Theta1_grad(:) ; Theta2_grad(:)];

end


function X = positive_definite_karcher_mean(A)
% Computes a Karcher mean of a collection of positive definite matrices.
%
% function X = positive_definite_karcher_mean(A)
%
% Input:  A 3D matrix A of size nxnxm such that each slice A(:,:,k) is a
%         positive definite matrix of size nxn.
%
% Output: A positive definite matrix X of size nxn which is a Karcher mean
%         of the m matrices in A, that is, X minimizes the sum of squared
%         Riemannian distances to the matrices in A:
%            f(X) = sum_k=1^m .5*dist^2(X, A(:, :, k))
%         The distance is defined by the natural metric on the set of
%         positive definite matrices: dist(X,Y) = norm(logm(X\Y), 'fro').
%
% This simple example is not the best way to compute Karcher means. Its
% purpose it to serve as base code to explore other algorithms. In
% particular, in the presence of large noise, this algorithm seems to not
% be able to reach points with a very small gradient norm. This may be
% caused by insufficient accuracy in the gradient computation.

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, Sept. 3, 2013
% Contributors:
%
% Change log:
%

    % Generate some random data to test the function if none is given.
    if ~exist('A', 'var') || isempty(A)
        n = 5;
        m = 10;
        A = zeros(n, n, m);
        ref = diag(max(.1, 1+.1*randn(n, 1)));
        for i = 1 : m
            noise = 0.01*randn(n);
            noise = (noise + noise')/2;
            [V D] = eig(ref + noise);
            A(:, :, i) = V*diag(max(.01, diag(D)))*V';
        end
    end

    % Retrieve the size of the problem:
    % There are m matrices of size nxn to average.
    n = size(A, 1);
    m = size(A, 3);
    assert(n == size(A, 2), ...
           ['The slices of A must be square, i.e., the ' ...
	        'first and second dimensions of A must be equal.']);

    % Our search space is the set of positive definite matrices of size n.
    % Notice that this is the only place we specify on which manifold we
    % wish to compute Karcher means. Replacing this factory for another
    % geometry will yield code to compute Karcher means on that other
    % manifold, provided that manifold is equipped with a dist function and
    % a logarithmic map log.
    M = sympositivedefinitefactory(n);

    % Define a problem structure, specifying the manifold M, the cost
    % function and its gradient.
    problem.M = M;
    problem.cost = @cost;
    problem.grad = @grad;

    % The functions below make many redundant computations. This
    % performance hit can be alleviated by using the caching system. We go
    % for a simple implementation here, as a tutorial example.

    % Cost function
    function f = cost(X)
        f = 0;
        for k = 1 : m
            f = f + M.dist(X, A(:, :, k))^2;
        end
        f = f/(2*m);
    end

    % Riemannian gradient of the cost function
    function g = grad(X)
        g = M.zerovec(X);
        for k = 1 : m
            % Update g in a linear combination of the form
            % g = g - [something]/m.
            g = M.lincomb(X, 1, g, -1/m, M.log(X, A(:, :, k)));
        end
    end

    % Execute some checks on the derivatives for early debugging.
    % These things can be commented out of course.
    % The slopes should agree on part of the plot at least. In this case,
    % it is sometimes necessary to inspect the plot visually to make the
    % call, but it is indeed correct.
    % checkgradient(problem);
    % pause;

    % Execute this if you want to force using a proper parallel vector
    % transport. This is not necessary. If you omit this, the default
    % vector transport is the identity map, which is (of course) cheaper
    % and seems to perform well in practice.
    % M.transp = M.paralleltransp;

    % Issue a call to a solver. Default options are selected.
    % Our initial guess is the first data point.
    X = trustregions(problem, A(:, :, 1));

end


function [gd,fnorm]=sgrpdlay(x,fnorm);
%SGRPDLAY Group delay estimation of a signal.
%	[GD,FNORM]=SGRPDLAY(X,FNORM) estimates the group delay of
%	signal X at the normalized frequency(ies) FNORM.
%
%	X     : signal in the time-domain.
%	FNORM : normalized frequency. By default, FNORM is a
%               linearly spaced vector between -0.5 and 0.5 with
%               length(X) elements.
%	GD    : computed group delay. When GD equals zero, it means that
%	 	the estimation of the group delay for this frequency was
%		outside the interval [1 xrow], and therefore meaningless.
%
%	Example :
%	 N=128; x=amgauss(N,64,30).*fmlin(N,0.1,0.4);
%	 fnorm=0.1:0.04:0.38; gd=sgrpdlay(x,fnorm);
%	 t=2:N-1; instf=instfreq(x,t);
%	 plot(t,instf,gd,fnorm); axis([1 N 0 0.5]);

%	F. Auger, March 1994, 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 == 0),
 error('At least one parameter required');
end;

[xrow,xcol] = size(x);
if (xcol~=1),
 error('x must have only one column');
end;
Ex=mean(abs(x).^2);
Threshold=1.0e-6;

if (nargin==1),
 Num=fft(x .* (1:xrow)');
 Den=fft(x);
 ratio=(real(Num./Den) .* (real(Num./Den)>=1) .* (real(Num./Den)<=xrow+3));
 gd=fftshift(ratio);
 if (nargout==2),
  if (rem(xrow,2)==0),
   L=xrow/2;
   fnorm=(-L:L-1)/xrow;
  else
   L=(xrow-1)/2;
   fnorm=(-L:L)/xrow;
  end;
 end;
elseif (nargin==2),
 [fnormrow,fnormcol] = size(fnorm);
 if (fnormrow~=1),
  error('FNORM must have only one row');
 end;
 expo=exp(-j*2.0*pi*fnorm'*(0:xrow-1));
 Num=expo*(x .* (1:xrow)');
 Den=expo*x;
 gd=(real(Num./Den) .* (real(Num./Den)>=1) .* (real(Num./Den)<=xrow+3));
end;


%%  Performing Gaussian blur
%   I: input image
%   s: standard deviation
%   GI: blurred image

function GI = gaussianBlur(I,s)

M = gaussianMask(1,s);
if max(size(M))==0
    GI=I;
    return;
end
M = M/sum(sum(M));   % normalize the gaussian mask
GI=I;
for i=1:(2*s+1)
    GI=BoundMirrorExpand(GI);
end
GI = conv2(GI,M,'same');
for i=1:(2*s+1)
    GI=BoundMirrorShrink(GI);
end


%DEMO_WAVELETS  Wavelet filter banks
%
%   This demo exemplifies the use of the wavelet filter bank trees. All
%   representations use "least asymmetric" Daubechies wavelet orthonormal
%   filters 'sym8' (8-regular, length 16).
%
%   .. figure::
%
%      DWT representation
%
%      The filter bank tree consists of 11 levels of iterated 2-band basic
%      wavelet filter bank, where only the low-pass output is further
%      decomposed. This results in 12 bands with octave resolution.
%
%   .. figure::
%
%      8-band DWT representation
%
%      The filter bank tree (effectively) consists of 3 levels of iterated
%      8-band basic wavelet filter bank resulting in 22 bands. Only the
%      low-pass output is decomposed at each level.
%
%   .. figure::
%
%      Full Wavelet filter bank tree representation
%
%      The filter bank tree depth is 8 and it is fully decomposed meaning
%      both outputs (low-pass and high-pass) of the basic filter bank is
%      plot further. This results in 256 bands linearly covering the
%      frequency axis.
%
%   .. figure::
%
%      Full Wavelet filter bank tree representation
%
%      The same case as before, but symmetric nearly orthogonal basic
%      filter bank is used.
%
%   .. figure::
%
%      Full Dual-tree Wavelet filter bank representation
%
%      This is a 2 times redundant representation using Q-shift dual-tree
%      wavelet filters.
%

% Read the test signal and crop it to the range of interest
[f,fs]=gspi;
f=f(10001:100000);
dr=50;

figure(1);
[c1,info]=fwt(f,'sym8',11);
plotwavelets(c1,info,fs,'dynrange',dr);

figure(2);
[c2,info]=wfbt(f,{'sym8',3,'quadband'});
plotwavelets(c2,info,fs,'dynrange',dr);

figure(3);
[c3,info]=wfbt(f,{'sym8',8,'full'});
plotwavelets(c3,info,fs,'dynrange',dr);

figure(4);
[c4,info]=wfbt(f,{'symorth3',8,'full'});
plotwavelets(c4,info,fs,'dynrange',dr);

figure(5);
[c5,info]=dtwfbreal(f,{'qshift5',8,'full','first','symorth3'});
plotwavelets(c5,info,fs,'dynrange',dr);


function pendulum()
% Matlab code for simulating a simple pendulum

P.g = 9.81;     %(m/s^2) gravity acceleration
P.l = 1.0;      %(m) length of pendulum
P.c = 1.0;     %(1/s) viscous damping constant
P.m = 1.0;     %(kg) pendulum mass

th0 = 0.5;  %(rad) initial angle of pendulum, from -j axis
w0 = 0.0;   %(rad/s) initial angular rate of pendulum
z0 = [th0;w0];   %Initial state vector

tSpan = [0,4]; %(s) [start, end] times for simulation
dtMax = 0.01;  %(s) maximum allowable time step

nStep = ceil(diff(tSpan)/dtMax);  %Number of simulation steps
t = linspace(tSpan(1),tSpan(2),nStep);  %(s) time vector
z = zeros(2,nStep); %  Initialize the state matrix
z(:,1) = z0;

%Run the simulation, using Euler integration
for i=2:nStep
    dt = t(i)-t(i-1);
    zNow = z(:,i-1);
    z(:,i) = zNow + dt*dynamics(zNow,P);
end

%Generate a plot of the result:
figure(1); clf;
subplot(2,1,1); plot(t,z(1,:));
xlabel('Time (s)'); ylabel('Angle (rad)');
title('Simple Damped Pendulum');
subplot(2,1,2); plot(t,z(2,:));
xlabel('Time (s)'); ylabel('Rate (rad/s)');

end

function dz = dynamics(z,P)
% Compute the dynamics of a simple damped pendulum:

th = z(1,:); w = z(2,:); %Unpack the state vector

%Dynamics:
dth = w;
dw = -(P.g/P.l)*sin(th) - (P.c/(P.m*P.l*P.l))*w;

dz = [dth; dw];  %Pack up state derivative
end