%%******************************************************************
%% geometric_mean: an example with rotated cone variables
%%
%%  max {prod(d+B*x) : d+B*x > 0, x <= 10}
%%
%%  where B = 4xn matrix,
%%        d = 4x1 vector
%%
%% [blk,At,C,b,xx] = geometric_mean(B,d,solve);
%%
%% E.g. p = 6; m = 4; B = [rand(2,p); -rand(2,p)]; d = rand(m,1);
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%******************************************************************

   function [blk,At,C,b,xx] = geometric_mean(B,d,solve);

   if (nargin == 2); solve = 0; end

   n = size(B,2);
   zz = zeros(1,n);
   r2 = sqrt(2);
   blk{1,1} = 'r'; blk{1,2} = [3,3,3];
   At{1,1} = -[B(1,:),0,0,0; B(2,:),0,0,0; zz,r2,0,0; ...
               B(3,:),0,0,0; B(4,:),0,0,0; zz,0,r2,0; ...
	       zz, 1,0,0; zz, 0,1,0; zz, 0,0,r2];
   C{1,1} = [d(1:2); 0; d(3:4); 0; 0;0;0];
   b = [zeros(n,1); 0;0;1];
   blk{2,1} = 'l'; blk{2,2} = n;
   At{2,1} = [eye(n), zeros(n,3)]; C{2,1} = 10*ones(n,1);
   if (solve)
      [bblk,AAt,CC,bb,T] = convertRcone(blk,At,C,b);
      [obj,X,y,Z] = sqlp(bblk,AAt,CC,bb);
      xx = y(1:n);
   end
%%******************************************************************


function [ a, seed ] = r8mat_orth_uniform ( n, seed )

%*****************************************************************************80
%
%% R8MAT_ORTH_UNIFORM returns a random orthogonal matrix.
%
%  Properties:
%
%    The inverse of A is equal to A'.
%
%    A * A'  = A' * A = I.
%
%    Columns and rows of A have unit Euclidean norm.
%
%    Distinct pairs of columns of A are orthogonal.
%
%    Distinct pairs of rows of A are orthogonal.
%
%    The L2 vector norm of A*x = the L2 vector norm of x for any vector x.
%
%    The L2 matrix norm of A*B = the L2 matrix norm of B for any matrix B.
%
%    The determinant of A is +1 or -1.
%
%    All the eigenvalues of A have modulus 1.
%
%    All singular values of A are 1.
%
%    All entries of A are between -1 and 1.
%
%  Discussion:
%
%    Thanks to Eugene Petrov, B I Stepanov Institute of Physics,
%    National Academy of Sciences of Belarus, for convincingly
%    pointing out the severe deficiencies of an earlier version of
%    this routine.
%
%    Essentially, the computation involves saving the Q factor of the
%    QR factorization of a matrix whose entries are normally distributed.
%    However, it is only necessary to generate this matrix a column at
%    a time, since it can be shown that when it comes time to annihilate
%    the subdiagonal elements of column K, these (transformed) elements of
%    column K are still normally distributed random values.  Hence, there
%    is no need to generate them at the beginning of the process and
%    transform them K-1 times.
%
%    For computational efficiency, the individual Householder transformations
%    could be saved, as recommended in the reference, instead of being
%    accumulated into an explicit matrix format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Pete Stewart,
%    Efficient Generation of Random Orthogonal Matrices With an Application
%    to Condition Estimators,
%    SIAM Journal on Numerical Analysis,
%    Volume 17, Number 3, June 1980, pages 403-409.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(N,N), the orthogonal matrix.
%
%    Output, integer SEED, a seed for the random number generator.
%
  a = zeros ( n, n );
%
%  Start with A = the identity matrix.
%
  for i = 1 : n
    for j = 1 : n
      if ( i == j )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end
    end
  end
%
%  Now behave as though we were computing the QR factorization of
%  some other random matrix.  Generate the N elements of the first column,
%  compute the Householder matrix H1 that annihilates the subdiagonal elements,
%  and set A := A * H1' = A * H.
%
%  On the second step, generate the lower N-1 elements of the second column,
%  compute the Householder matrix H2 that annihilates them,
%  and set A := A * H2' = A * H2 = H1 * H2.
%
%  On the N-1 step, generate the lower 2 elements of column N-1,
%  compute the Householder matrix HN-1 that annihilates them, and
%  and set A := A * H(N-1)' = A * H(N-1) = H1 * H2 * ... * H(N-1).
%  This is our random orthogonal matrix.
%
  for j = 1 : n-1
%
%  Set the vector that represents the J-th column to be annihilated.
%
    x(1:j-1) = 0.0;

    for i = j : n
      [ x(i), seed ] = r8_normal_01 ( seed );
    end
%
%  Compute the vector V that defines a Householder transformation matrix
%  H(V) that annihilates the subdiagonal elements of X.
%
    v = r8vec_house_column ( n, x, j );
%
%  Postmultiply the matrix A by H'(V) = H(V).
%
    a = r8mat_house_axh ( n, a, v );

  end

  return
end


function i4_factorial2_values_test ( )

%*****************************************************************************80
%
%% I4_FACTORIAL2_VALUES_TEST demonstrates the use of I4_FACTORIAL2_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4_FACTORIAL2_VALUES_TEST:\n' );
  fprintf ( 1, '  I4_FACTORIAL2_VALUES returns values of\n' );
  fprintf ( 1, '  the double factorial function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N         N!!\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, fn ] = i4_factorial2_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %4d  %10d\n',  n, fn );

  end

  return
end


function [b,a]=v_potsband(fs)
%V_POTSBAND Design filter for 300-3400 telephone bandwidth [B,A]=(FS)
%
%Input: FS=sample frequency in Hz
%
%Output: B/A is a discrete time bandpass filter with a passband gain of 1
%
%The filter meets the specifications of G.151 for any sample frequency
%and has a gain of -3dB at the passband edges.

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_potsband.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

szp=[0.19892796195357i; -0.48623571568937+0.86535995266875i];
szp=[[0; -0.97247143137874] szp conj(szp)];
% s-plane zeros and poles of high pass 3'rd order chebychev2 filter with -3dB at w=1
zl=2./(1-szp*tan(300*pi/fs))-1;
al=real(poly(zl(2,:)));
bl=real(poly(zl(1,:)));
sw=[1;-1;1;-1];
bl=bl*(al*sw)/(bl*sw);
zh=2./(szp/tan(3400*pi/fs)-1)+1;
ah=real(poly(zh(2,:)));
bh=real(poly(zh(1,:)));
bh=bh*sum(ah)/sum(bh);
b=conv(bh,bl);
a=conv(ah,al);


%#eml
function R = radix4FFT3_FixPtEML(s)
    % This is a radix-4 FFT, using decimation in frequency
    % The input signal can be floating point or fixed point
    % Works with real or complex input
    % Modified for Embedded MATLAB support

    % Initialize variables and signals
    % NOTE: The length of the input signal should be a power of 4: 4, 16, 64, 256, etc.
    N = length(s);
    M = log2(N)/2;

    % Initialize variables for floating or fixed point sim
    if isfi(s)
        NT = numerictype(s);
        FM = fimath(s);
        wl = NT.WordLength;
        W=fi(exp(-1*2j*pi*(0:N-1)/N), 1, wl, wl - 2, FM);
        S = fi(complex(zeros(size(s))), NT, FM);
        R = fi(complex(zeros(size(s))), 1, wl, wl  - 1 - 2*M, FM);
        sTemp = fi(complex(zeros(size(s))), NT, FM);
        x = fi(complex(zeros(1,4)), NT, FM);
    else
        W=exp(-1*2j*pi*(0:N-1)/N);
        S = complex(zeros(size(s)));
        R = complex(zeros(size(s)));
        sTemp = complex(zeros(size(s)));
        x = complex(zeros(1,4));
    end

    % FFT algorithm
    % Calculate butterflies for first M-1 stages
    sTemp = s;
    for stage = 0:M-2
        for n=1:N/4
            for m=1:4
                x(m) = sTemp(n + (m-1)*N/4);
            end
            S((1:4)+(n-1)*4) = radix4bfly(x, floor((n-1)/(4^stage)) *(4^stage), 1, W);
        end
        sTemp = S;
    end

    % Calculate butterflies for last stage
    for n=1:N/4
        for m=1:4
            x(m) = sTemp(n + (m-1)*N/4);
        end
        S((1:4)+(n-1)*4) = radix4bfly(x, floor((n-1)/(4^stage)) * (4^stage), 0, W);
    end

    % Rescale the final output
    R(:) = S*N;

end

function Z = radix4bfly(x,segment,stageFlag,W)
    % For the last stage of a radix-4 FFT all the ABCD multiplers are 1.
    % Use the stageFlag variable to indicate the last stage
    % stageFlag = 0 indicates last FFT stage, set to 1 otherwise

    % Initialize variables and scale by 1/4
    if isfi(x)
        a=bitshift(x(1),-2);b=bitshift(x(2),-2);c=bitshift(x(3),-2);d=bitshift(x(4),-2);
    else
        a=x(1)*.25;b=x(2)*.25;c=x(3)*.25;d=x(4)*.25;
    end

    % Radix-4 Algorithm
    A=a+b+c+d;
    B=(a-b+c-d)*W(2*segment*stageFlag + 1);
    C=(a-complex(-imag(b),real(b))-c+complex(-imag(d),real(d)))*W(segment*stageFlag + 1);
    D=(a+complex(-imag(b),real(b))-c-complex(-imag(d),real(d)))*W(3*segment*stageFlag + 1);

    Z = [A B C D];

end


function [xUpdate,PUpdate,innov,Pzz,W]=KalmanUpdate(xPred,PPred,z,R,H,condIssue)
%KALMANUPDATE Perform the measurement update step in the standard linear
%             Kalman filter. This assumes that the predicted target state
%             and the measurement are both multivariate Gaussian
%             distributed.
%
%INPUTS: xPred The xDimX1 predicted target state.
%        PPred The xDimXxDim predicted state covariance matrix.
%            z The zDimX1 measurement vector.
%            R The zDimXzDim measurement covariance matrix.
%            H The optional zDimXxDim measurement matrix. The measurement
%              is modeled as z=H*x+noise. If this parameter is omitted or
%              an empty matrix is passed, then H will be taken as a
%              zDimXzDim identity matrix followed by columns of zeros
%              (Assuming that zDim<=xDim. Otherwise, H must be provided).
%    condIssue An optional parameter indicating whether a conditioning
%              issue with the measurement covariance matrix. Singularity
%              can occur if two elements are perfectly correlated. This
%              replaces the matrix inverse with a pseudoinverse.
%
%OUTPUTS: xUpdate The xDim X 1 updated target state vector.
%         PUpdate The updated xDim X xDim state covariance matrix.
%      innov, Pzz The zDimX1 innovation and the zDimXzDim innovation
%                 covariance matrix are returned in case one wishes to
%                 analyze the consistency of the estimator or use those
%                 values in gating or likelihood evaluation.
%               W The xDimXzDim gain used in the update. This can be
%                 useful when gating and using the function
%                 calcMissedGateCov.
%
%Given a prediction of the target state xPred with an associated covariance
%matrix PPred, and assuming that they are the mean and covariance matrix of
%a Gaussian distribution, under the measurement model
%z=H*x+w
%where z is the measurement, x is the true target state, H is a zDimXxDim
%matrix and w is additive Gaussian noise, this function updates the state
%estimate and its associated covariance estimate.
%
%The Joseph-form covariance update is used for improved numerical
%stability. The algorithm is derived in Chapter 5 of [1].
%
%EXAMPLE:
%This example simulates a linear dynamic process that extracts Cartesian
%measurements. it then looks at the measurement error reduction factor
%(MERF), and the normalized estimation error squared (NEES). The MERF
%should be less than 1 or else there is no point in using the filter-
%connecting the dots works better. The NEES should be close to 1 to
%indicate covariance consistency.
% numMonteCarlo=500;
% numSteps=50;
% %Power spectral density of the process noise.
% q0=processNoiseSuggest('PolyKal-ROT',2*9.8,1);
% xDim=4;%2D position and velocity.
% T=1;%prediction interval.
% F=FPolyKal(T,xDim,1);
% Q=QPolyKal(T,xDim,1,q0);
% SQ=chol(Q,'lower');
% R=[50^2,-1000;
%    -1000,50^2];
% SR=chol(R,'lower');
% H=[eye(2,2),zeros(2,2)];
% zDim=size(H,1);
%
% z=zeros(zDim,numSteps,numMonteCarlo);
% xTrue=zeros(xDim,numSteps,numMonteCarlo);
% %The initial state.
% xTrue(:,1,:)=repmat([1e3;0;75;-50],[1,1,numMonteCarlo]);
% maxVel=400;%Used for single-point initiation.
% higherDerivStdDev=maxVel/sqrt(2);%used for single-point initiation.
%
% xEst=zeros(xDim,numSteps,numMonteCarlo);
% PEst=zeros(xDim,xDim,numSteps,numMonteCarlo);
% for curRun=1:numMonteCarlo
%     z(:,1,curRun)=H*xTrue(:,1,curRun)+SR*randn(zDim,1);
%     for curStep=2:numSteps
%         xTrue(:,curStep,curRun)=F*xTrue(:,curStep-1,curRun)+SQ*randn(xDim,1);
%         z(:,curStep,curRun)=H*xTrue(:,curStep,curRun)+SR*randn(zDim,1);
%     end
%
%     %One-point initialization. This works in this instance, because H
%     %extracts the Cartesian components.
%     [xInit,PInit]=onePointCartInit(z(:,1,curRun),R,higherDerivStdDev,1);
%     xEst(:,1,curRun)=xInit;
%     PEst(:,:,1,curRun)=PInit;
%     for curStep=2:numSteps
%         [xPred,PPred]=discKalPred(xEst(:,curStep-1,curRun,1),PEst(:,:,curStep-1,curRun,1),F,Q);
%         [xUpdate,PUpdate]=KalmanUpdate(xPred,PPred,z(:,curStep,curRun),R,H);
%         xEst(:,curStep,curRun)=xUpdate;
%         PEst(:,:,curStep,curRun)=PUpdate;
%     end
% end
%
% posTrue=xTrue(1:2,:,:);
% %The MERF requires the measurement to be in the same coordinates are the
% %corresponding element sof the state.
% MERF=calcMERF(posTrue,z,xEst(1:2,:,:),0,true);
% NEES=calcNEES(xTrue,xEst,PEst);
% figure(1)
% clf
% hold on
% plot(MERF,'-k','linewidth',2)
% h1=xlabel('step');
% h2=ylabel('Measurement Error Reduction Factor');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
% figure(2)
% clf
% hold on
% plot(NEES,'-r','linewidth',2)
% h1=xlabel('step');
% h2=ylabel('NEES');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%
%July 2012 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xDim=size(xPred,1);

if(nargin<6||isempty(condIssue))
    condIssue=false;
end

if(nargin<5||isempty(H))
	zDim=size(z,1);
    H=[eye(zDim,zDim),zeros(zDim,xDim-zDim)];
end

zPred=H*xPred;
innov=z-zPred;

Pzz=R+H*PPred*H';
%Ensure symmetry
Pzz=(Pzz+Pzz')/2;

if(condIssue==false)
    opts.SYM=true;
    opts.RECT=false;
    opts.TRANSA=false;
    W=linsolve(Pzz,H*PPred,opts)';%W=PPred*H'/Pzz;
else
    %If there is possibly poor conditioning.
    W=PPred*H'*pinv(Pzz);
end

xUpdate=xPred+W*innov;

temp=W*H;
temp=eye(xDim,xDim)-temp;
PUpdate=temp*PPred*temp'+W*R*W';

%Ensure symmetry
PUpdate=(PUpdate+PUpdate')/2;
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 idxSelection = uniformSampling(X, voxelSize)

% Preparations -----------------------------------------------------------------

% No. of points
noPoi = size(X,1);

% Logical indices for selection of points (true = selected)
idxSelection = false(noPoi,1);

% Find voxel centers -----------------------------------------------------------

% Point with smallest coordinates
minPoi = min(X, [], 1);

% Rounded local origin for voxel structure (voxels of different pcs have coincident voxel centers if mod(100, voxelSize) == 0)
localOrigin = (floor(minPoi/100))*100;

% Find 3-dimensional indices of voxels in which points are lying
idxVoxel = [floor( (X(:,1)-localOrigin(1)) / voxelSize ) ...
            floor( (X(:,2)-localOrigin(2)) / voxelSize ) ...
            floor( (X(:,3)-localOrigin(3)) / voxelSize )];

% Remove multiple voxels
[idxVoxelUnique, ~, ic] = unique(idxVoxel, 'rows'); % ic contains "voxel index" for each point

% Coordinates of voxel centers
XVoxelCenter = [localOrigin(1) + voxelSize/2 + idxVoxelUnique(:,1)*voxelSize ...
                localOrigin(2) + voxelSize/2 + idxVoxelUnique(:,2)*voxelSize ...
                localOrigin(3) + voxelSize/2 + idxVoxelUnique(:,3)*voxelSize];

% No. of voxel (equal to no. of selected points)
noVoxel = size(XVoxelCenter,1);

% Select points nearest to voxel centers ---------------------------------------

% Sort indices and points (in order to find points inside of voxels very fast in the next loop)
[ic, idxSort] = sort(ic);
X = X(idxSort,:);

idxJump = find(diff(ic));

% Example (3 voxel)
% ic         = [1 1 1 2 2 2 3]';
% diff(ic)   = [ 0 0 1 0 0 1 ]';
% idxJump    = [     3     6 ]';
%
% idxInVoxel = [1 2 3]; for voxel 1
% idxInVoxel = [4 5 6]; for voxel 2
% idxInVoxel = [7    ]; for voxel 3

for i = 1:noVoxel

    % Find indices of points inside of voxel (very, very fast this way)
    if i == 1
        idxInVoxel = [1:idxJump(i)];
    elseif i == noVoxel
        idxInVoxel = [idxJump(i-1)+1:noPoi];
    else
        idxInVoxel = [idxJump(i-1)+1:idxJump(i)];
    end

    % Distance of points to voxel center
    dist2voxelCenter = sqrt((X(idxInVoxel,1)-XVoxelCenter(i,1)).^2 + (X(idxInVoxel,2)-XVoxelCenter(i,2)).^2 + (X(idxInVoxel,3)-XVoxelCenter(i,3)).^2);

    % Find index of point with smallest distance to voxel center
    [~, idxSelectedPoi] = min(dist2voxelCenter);

    % Select point
    idxSelection(idxSort(idxInVoxel(idxSelectedPoi))) = true;

    % 4Debug
    % idxInVoxel
    % idxSelectedPoi
    % XVoxelCenter(i,:)
    % X(idxInVoxel,:)
    % dist2voxelCenter
    % maxDist = sqrt(3*(voxelSize/2)^2);
    % correct(i) = all(dist2voxelCenter <= maxDist);

end

end


function [ vX, mX ] = SolveLsL2Prox( mA, vB, paramLambda, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX, mX ] = SolveLsL1Prox( mA, vB, lambdaFctr, numIterations )
% Solve L2 Regularized Least Squares Using Proximal Gradient (PGM) Method.
% Input:
%   - mA                -   Input Matirx.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vB                -   input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L1 Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia PGM - https://en.wikipedia.org/wiki/Proximal_gradient_method.
% Remarks:
%   1.  Using vanilla PGM.
% Known Issues:
%   1.  A
% TODO:
%   1.  B
% Release Notes:
%   -   1.0.000     23/08/2017
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

mAA = mA.' * mA;
vAb = mA.' * vB;
vX  = pinv(mA) * vB; %<! Dealing with "Fat Matrix"

stepSize = 1 / (2 * (norm(mA, 2) ^ 2));
% stepSize = 1 / sum(mA(:) .^ 2); %<! Faster to calculate, conservative (Hence slower)

mX = zeros([size(vX, 1), numIterations]);
mX(:, 1) = vX;

for ii = 2:numIterations

    vG = (mAA * vX) - vAb;
    vX = ProxL2(vX - (stepSize * vG), stepSize * paramLambda);

    mX(:, ii) = vX;

end

end

function [ vX ] = ProxL2( vX, lambdaFactor )

% Soft Thresholding L2
% See https://math.stackexchange.com/a/2094088/33
vX = (1 - (lambdaFactor / max(norm(vX, 2), lambdaFactor))) * vX;

end


function [ result, node_num ] = sphere01_triangle_quad_icos2v ( a_xyz, ...
  b_xyz, c_xyz, factor, fun )

%*****************************************************************************80
%
%% SPHERE01_TRIANGLE_QUAD_ICOS2V: vertex rule, subdivide then project.
%
%  Discussion:
%
%    This function estimates an integral over a spherical triangle on the
%    unit sphere.
%
%    This function sets up an icosahedral grid, and subdivides each
%    edge of the icosahedron into FACTOR subedges.  These edges define a grid
%    within each triangular icosahedral face.   All of these calculations are
%    done, essentially, on the FLAT faces of the icosahedron.  Only then are
%    the triangle vertices projected to the sphere.
%
%    This function uses a more sophisticated projection scheme than
%    SPHERE01_TRIANGLE_QUAD_ICOS1V, but this does not seem to improve
%    the results significantly.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A_XYZ(3), B_XYZ(3), C_XYZ(3), the vertices
%    of the spherical triangle.
%
%    Input, integer FACTOR, the subdivision factor, which must
%    be at least 1.
%
%    Input, function v = FUN ( x ), evaluates the integrand at the point X.
%
%    Output, real RESULT, the estimated integral.
%
%    Output, integer NODE_NUM, the number of evaluation points.
%

%
%  Initialize the integral data.
%
  result = 0.0;
  area_total = 0.0;
  node_num = 0;
%
%  Deal with subtriangles that have same orientation as face.
%
  for f1 = 0 : factor - 1
    for f2 = 0 : factor - f1 - 1
      f3 = factor - f1 - f2;

      a2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1 + 1, f2,     f3 - 1 );
      b2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1,     f2 + 1, f3 - 1 );
      c2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1,     f2,     f3 );

      area = sphere01_triangle_vertices_to_area ( a2_xyz, b2_xyz, c2_xyz );

      node_num = node_num + 3;
      va = fun ( a2_xyz );
      vb = fun ( b2_xyz );
      vc = fun ( c2_xyz );
      result = result + area * ( va + vb + vc ) / 3.0;
      area_total = area_total + area;

      end
    end
%
%  Deal with subtriangles that have opposite orientation as face.
%
  for f3 = 0 : factor - 2
    for f2 = 1 : factor - f3 - 1
      f1 = factor - f2 - f3;

      a2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1 - 1, f2,     f3 + 1 );
      b2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1,     f2 - 1, f3 + 1 );
      c2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1,     f2,     f3 );

      area = sphere01_triangle_vertices_to_area ( a2_xyz, b2_xyz, c2_xyz );

      node_num = node_num + 3;
      va = fun ( a2_xyz );
      vb = fun ( b2_xyz );
      vc = fun ( c2_xyz );
      result = result + area * ( va + vb + vc ) / 3.0;
      area_total = area_total + area;

    end
  end

  return
end


function [slope, intercept, averslope, averIC] = fdsurfft(im)
% FDSURFFT Compute fractal dimension (slope) of surface image im and draw rose plots
%          of slope and intercept
%     im: input array of surface image (grayvalue or range image)
%     slope: an array of size 24 which stores the average slopes in 24
%            directions
%     intercept: an array of size 24 which stores the average intercepts in 24
%                directions
%     averslope: average slope for all directions
%     averIC: average intercept for all directions

% This is a matlab version of John C. Russ 's program

% Written by Mr. Jianbo Zhang, J.Zhang@ewi.utwente.nl
% 2 Nov, 2004

tic
NUM_DIR = 24; % number of directions that the frequency space is uniformally divided
NUM_RAD = 30; % number of points that the radius is uniformally divided
if nargin < 1,
    error('Missed input argument which must be an array!')
end

[M N] = size(im);
xctr = 1 + bitshift(N, -1); % x coordinate of center point
yctr = 1 + bitshift(M, -1); % y coordinate of center point
imMean = mean(im(:));
fim = fftshift(fft2(double(im) - imMean));

% power spectrum
mag = log(fim .* conj(fim)+ 10 ^ (-6));
sumBrite = zeros(NUM_DIR, NUM_RAD); %accumulation magnitude for each direction and radius
nCount = zeros(NUM_DIR, NUM_RAD); %number of magnitude
radius = zeros(2 * NUM_RAD,1);%accumulation magnitude for all directions
radCount = zeros(2 * NUM_RAD,1);% number of magintude for all directions

%Compute phase image and phase histogram
phaseim = zeros(M,N);
phase = zeros(180);
for j = 1:M
    for i = 1:N
        realv = real(fim(j,i));
        imagv = imag(fim(j,i));
        if realv == 0
            value = pi/2;
        else
            value = atan((imagv / realv));
            phaseim(j, i) = value;
            ang = floor(180 * (pi / 2 + value) / pi);
        end
        if ang < 0
            ang = 0;
        end
        if ang > 179
            ang = 179;
        end
        phase(ang + 1) = phase(ang + 1) + 1;
    end
end
maxphase = max(phase);
figure; imshow(phaseim, []);
title('Phase image');
figure; plot(phase / maxphase);
title('Phase histogram (0...2 \pi)');%
axis off

%accumulation of magnitude for each dirction and radius
rmax = log(min(M,N)/2);% maximum radius
for j = 1:M
    if j ~= yctr
        yval = yctr - j;
        y2 = yval * yval;
        for i = 1:N
            if i ~= xctr
                xval = i - xctr;
                rho = log(sqrt(y2 + xval * xval));
                if rho > 0 & rho <= rmax
                    mval = mag(j,i);
                    temp = yval /xval;
                    theta = atan(temp);
                    if xval < 0
                        theta = theta + pi;
                    end
                    if theta < 0
                        theta = theta + 2 * pi;
                    end

                    ang = floor(NUM_DIR * theta /(2* pi));
                    if ang > NUM_DIR - 1 | ang < 0
                        ang = NUM_DIR - 1;
                    end
                    k = floor(2*NUM_RAD * rho / rmax);
                    h = floor(k/2);
                    if k > 2 * NUM_RAD - 1
                        h = NUM_RAD - 1;
                        k = 2 * NUM_RAD - 1;
                    end

                    if h >= 5
                        sumBrite(ang + 1, h + 1) = sumBrite(ang + 1, h + 1) + mval;
                        nCount(ang + 1, h + 1) = nCount(ang + 1, h + 1) + 1;
                    end
                    if k >= 5
                        radius(k + 1) = radius(k + 1) + mval;
                        radCount(k + 1) = radCount(k + 1) + 1;
                    end
                end
            end
        end
    end
end

%linear regression
for ang = 1:NUM_DIR
    sumx = 0;
    sumy = 0;
    sumx2 = 0;
    sumxy = 0;
    sumn = 0;
    for range = 6:NUM_RAD
        if nCount(ang, range) > 0
            yval = sumBrite(ang, range)/nCount(ang, range);
            xval = (range -1) * rmax / NUM_RAD;
            sumx = sumx + xval;
            sumy = sumy + yval;
            sumx2 = sumx2 + xval * xval;
            sumxy = sumxy + xval * yval;
            sumn = sumn + 1;
        end
    end
    slope(ang) = (sumn * sumxy - sumx * sumy) / (sumn * sumx2 - sumx * sumx);
    intercept(ang) = (sumy - slope(ang) * sumx) / sumn;
end

%compute average slope over all directions and scales
sumn = 0;
for k = 6:(2 * NUM_RAD)
    if radCount(k) > 0
        sumn = sumn + 1;
        yval(sumn) = radius(k) / radCount(k);
        tempr(sumn) = (k -1) * rmax / (2 * NUM_RAD);
    end
end
p = polyfit(tempr,yval,1);
averslope = p(1);
averIC = p(2);

fitln = polyval(p,tempr);
figure; plot(tempr,yval,tempr,fitln,'r-');
title('Log Log plot of Magn. vs Freq.');
ylabel('Log Magnitude');
xlabel('Log Frequency');

slope(NUM_DIR + 1) = slope(1);
intercept(NUM_DIR + 1) = intercept(1);

%draw rose plot of slope and intercept
ang = 1: (NUM_DIR + 1);
figure;
polar(pi/NUM_DIR + (ang -1) * 2* pi / NUM_DIR, intercept(ang), 'r-');
title('Rose plot of intercept');
figure;
polar(pi/NUM_DIR + (ang - 1)* 2 * pi / NUM_DIR, abs(slope(ang)), '-');
title('Rose plot of slope');
disp(['Elapsed time: ' num2str(toc)]);


function [flag, u, v, t] = rayTriangleIntersection (o, d, p0, p1, p2)
% Ray/triangle intersection using the algorithm proposed by Möller and Trumbore (1997).
%
% Input:
%    o : origin.
%    d : direction.
%    p0, p1, p2: vertices of the triangle.
% Output:
%    flag: (0) Reject, (1) Intersect.
%    u,v: barycentric coordinates.
%    t: distance from the ray origin.
% Author:
%    Jesus Mena

    epsilon = 0.00001;

    e1 = p1-p0;
    e2 = p2-p0;
    q  = cross(d,e2);
    a  = dot(e1,q); % determinant of the matrix M

    if (a>-epsilon && a<epsilon)
        % the vector is parallel to the plane (the intersection is at infinity)
        [flag, u, v, t] = deal(0,0,0,0);
        return;
    end;

    f = 1/a;
    s = o-p0;
    u = f*dot(s,q);

    if (u<0.0)
        % the intersection is outside of the triangle
        [flag, u, v, t] = deal(0,0,0,0);
        return;
    end;

    r = cross(s,e1);
    v = f*dot(d,r);

    if (v<0.0 || u+v>1.0)
        % the intersection is outside of the triangle
        [flag, u, v, t] = deal(0,0,0,0);
        return;
    end;

    t = f*dot(e2,r); % verified!
    flag = 1;
    return;
end


function x = dipole_cdf_inv ( cdf, a, b )

%*****************************************************************************80
%
%% DIPOLE_CDF_INV inverts the Dipole CDF.
%
%  Discussion:
%
%    A simple bisection method is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%
%    Input, real A, B, the parameters of the PDF.
%    -1.0 <= B <= 1.0.
%
%    Output, real X, the corresponding argument of the CDF.
%
  it_max = 100;
  tol = 0.0001;
%
%  Take care of horrible input.
%
  if ( cdf <= 0.0 )
    x = -r8_huge ( )
    return
  elseif ( 1.0 <= cdf )
    x = r8_huge ( )
    return
  end
%
%  Seek X1 < X < X2.
%
  x1 = -1.0;

  while ( 1 )

    cdf1 = dipole_cdf ( x1, a, b );

    if ( cdf1 <= cdf )
      break
    end

    x1 = 2.0 * x1;

  end

  x2 = 1.0;

  while ( 1 )

    cdf2 = dipole_cdf ( x2, a, b );

    if ( cdf <= cdf2 )
      break
    end

    x2 = 2.0 * x2;

  end
%
%  Now use bisection.
%
  it = 0;

  while ( 1 )

    it = it + 1;

    x3 = 0.5 * ( x1 + x2 );
    cdf3 = dipole_cdf ( x3, a, b );

    if ( abs ( cdf3 - cdf ) < tol )
      x = x3;
      break
    end

    if ( it_max < it )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'DIPOLE_CDF_INV - Fatal error!\n' );
      fprintf ( 1, '  Iteration limit exceeded.\n' );
      error ( 'DIPOLE_CDF_INV - Fatal error!' );
    end

    if ( ( cdf3 <= cdf & cdf1 <= cdf ) | ( cdf <= cdf3 & cdf <= cdf1 ) )
      x1 = x3;
      cdf1 = cdf3;
    else
      x2 = x3;
      cdf2 = cdf3;
    end

  end

  return
end


%
% References:
%
% C. Lu. A Library of ADMM for Sparse and Low-rank Optimization. National University of Singapore, June 2016.
% https://github.com/canyilu/LibADMM.
% C. Lu, J. Feng, S. Yan, Z. Lin. A Unified Alternating Direction Method of Multipliers by Majorization
% Minimization. IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 40, pp. 527-541, 2018
%

addpath(genpath(cd))
clear

%% Examples for testing the low-rank tensor models
% For detailed description of the sparse models, please refer to the Manual.

opts.mu = 1e-6;
opts.rho = 1.1;
opts.max_iter = 500;
opts.DEBUG = 1;

%% Tensor RRPCA based on sum of nuclear norm minimization (rpca_snn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 5
L = rand(r,r,r);
U1 = rand(n1,r);
U2 = rand(n2,r);
U3 = rand(n3,r);
L = nmodeproduct(L,U1,1);
L = nmodeproduct(L,U2,2);
L = nmodeproduct(L,U3,3); % low rank part

p = 0.05;
m = p*n1*n2*n3;
temp = rand(n1*n2*n3,1);
[~,I] = sort(temp);
I = I(1:m);
Omega = zeros(n1,n2,n3);
Omega(I) = 1;
E = sign(rand(n1,n2,n3)-0.5);
S = Omega.*E; % sparse part, S = P_Omega(E)

Xn = L+S;

lambda = sqrt([max(n1,n2*n3), max(n2,n1*n3), max(n3,n1*n2)]);
lambda = [1 1 1]
[Lhat,Shat,err,iter] = trpca_snn(Xn,lambda,opts);

err
iter

%% low-rank tensor completion based on sum of nuclear norm minimization (lrtc_snn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 5;
X = rand(r,r,r);
U1 = rand(n1,r);
U2 = rand(n2,r);
U3 = rand(n3,r);
X = nmodeproduct(X,U1,1);
X = nmodeproduct(X,U2,2);
X = nmodeproduct(X,U3,3);
p = 0.5;
omega = find(rand(n1*n2*n3,1)<p);
M = zeros(n1,n2,n3);
M(omega) = X(omega);

lambda = [1 1 1];
[Xhat,err,iter] = lrtc_snn(M,omega,lambda,opts);
err
iter
RSE = norm(X(:)-Xhat(:))/norm(X(:))

%% regularized low-rank tensor completion based on sum of nuclear norm minimization (lrtcR_snn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 5;
X = rand(r,r,r);
U1 = rand(n1,r);
U2 = rand(n2,r);
U3 = rand(n3,r);
X = nmodeproduct(X,U1,1);
X = nmodeproduct(X,U2,2);
X = nmodeproduct(X,U3,3);
p = 0.5;
omega = find(rand(n1*n2*n3,1)<p);
M = zeros(n1,n2,n3);
M(omega) = X(omega);
lambda = [1 1 1];
[Xhat,err,iter] = lrtcR_snn(M,omega,lambda,opts);
err
iter

%% Tensor RRPCA based on tensor nuclear norm minimization (rpca_tnn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 0.1*n1 % tubal rank
L1 = randn(n1,r,n3)/n1;
L2 = randn(r,n2,n3)/n2;
L = tprod(L1,L2); % low rank part

p = 0.1;
m = p*n1*n2*n3;
temp = rand(n1*n2*n3,1);
[~,I] = sort(temp);
I = I(1:m);
Omega = zeros(n1,n2,n3);
Omega(I) = 1;
E = sign(rand(n1,n2,n3)-0.5);
S = Omega.*E; % sparse part, S = P_Omega(E)

Xn = L+S;
lambda = 1/sqrt(n3*max(n1,n2));

tic
[Lhat,Shat] = trpca_tnn(Xn,lambda,opts);

RES_L = norm(L(:)-Lhat(:))/norm(L(:))
RES_S = norm(S(:)-Shat(:))/norm(S(:))
trank = tubalrank(Lhat)

%% low-rank tensor completion based on tensor nuclear norm minimization (lrtc_tnn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 0.1*n1 % tubal rank
L1 = randn(n1,r,n3)/n1;
L2 = randn(r,n2,n3)/n2;
X = tprod(L1,L2); % low rank part
p = 0.5;
omega = find(rand(n1*n2*n3,1)<p);
M = zeros(n1,n2,n3);
M(omega) = X(omega);

[Xhat,obj,err,iter] = lrtc_tnn(M,omega,opts);

err
iter
RSE = norm(X(:)-Xhat(:))/norm(X(:))
trank = tubalrank(Xhat)

%% regularized low-rank tensor completion based on tensor nuclear norm minimization (lrtcR_tnn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 0.1*n1 % tubal rank
L1 = randn(n1,r,n3)/n1;
L2 = randn(r,n2,n3)/n2;
X = tprod(L1,L2); % low rank part
p = 0.5;
omega = find(rand(n1*n2*n3,1)<p);
M = zeros(n1,n2,n3);
M(omega) = X(omega);

lambda = 0.5;
[Xhat,Ehat,obj,err,iter] = lrtcR_tnn(M,omega,lambda,opts);
err
iter

%% low-rank tensor recovery from Gaussian measurements based on tensor nuclear norm minimization (lrtr_Gaussian_tnn)
n1 = 30;
n2 = n1;
n3 = 5;
r = 0.2*n1; % tubal rank
X = tprod(randn(n1,r,n3),randn(r,n2,n3)); % size: n1*n2*n3

m = 3*r*(n1+n2-r)*n3+1; % number of measurements
n = n1*n2*n3;
A = randn(m,n)/sqrt(m);

b = A*X(:);
Xsize.n1 = n1;
Xsize.n2 = n2;
Xsize.n3 = n3;

opts.DEBUG = 1;
[Xhat,obj,err,iter]  = lrtr_Gaussian_tnn(A,b,Xsize,opts);

RSE = norm(Xhat(:)-X(:))/norm(X(:))
trank = tubalrank(Xhat)


function geometry_test044 ( )

%*****************************************************************************80
%
%% TEST044 tests SEGMENTS_INT_1D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 April 2009
%
%  Author:
%
%    John Burkardt
%
  test_num = 7;

  q1_test = [ -1.0, 3.0, 1.0,  0.5, 0.25, 0.5, 2.0 ];
  q2_test = [  1.0, 2.0, 2.0, -3.0, 0.50, 0.5, 2.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST044\n' );
  fprintf ( 1, '  SEGMENTS_INT_1D determines the intersection [R1,R2]\n' );
  fprintf ( 1, '  of line segments [P1,P2] and [Q1,Q2] in 1D.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  DIST is negative for overlap,\n' );
  fprintf ( 1, '  0 for point intersection,\n' );
  fprintf ( 1, '  positive if there is no overlap.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '  Test      P1        P2        Q1        Q2        R1        R2        DIST\n' );
  fprintf ( 1, '\n' );

  p1 = -1.0;
  p2 = 1.0;

  for test = 1 : test_num

    q1 = q1_test(test);
    q2 = q2_test(test);

    [ dist, r1, r2 ] = segments_int_1d ( p1, p2, q1, q2 );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %4d  %8f  %8f  %8f  %8f  %8f  %8f  %8f\n', ...
      test, p1, p2, q1, q2, r1, r2, dist );

  end

  return
end


function bvec = bvec_next_grlex ( n, bvec )

%*****************************************************************************80
%
%% BVEC_NEXT_GRLEX generates the next binary vector in GRLEX order.
%
%  Discussion:
%
%    N = 3
%
%    Input      Output
%    -----      ------
%    0 0 0  =>  0 0 1
%    0 0 1  =>  0 1 0
%    0 1 0  =>  1 0 0
%    1 0 0  =>  0 1 1
%    0 1 1  =>  1 0 1
%    1 0 1  =>  1 1 0
%    1 1 0  =>  1 1 1
%    1 1 1  =>  0 0 0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension.
%
%    Input, integer BVEC(N), the binary vector whose successor is desired.
%
%    Output, integer BVEC(N), the successor to the input vector.
%

%
%  Initialize locations of 0 and 1.
%
  if ( bvec(1) == 0 )
    z = 1;
    o = 0;
  else
    z = 0;
    o = 1;
  end
%
%  Moving from right to left, search for a "1", preceded by a "0".
%
  for i = n : -1 : 2
    if ( bvec(i) == 1 )
      o = i;
      if ( bvec(i-1) == 0 )
        z = i - 1;
        break
      end
    end
  end
%
%  BVEC = 0
%
  if ( o == 0 )
    bvec(n) = 1;
%
%  01 never occurs.  So for sure, B(1) = 1.
%
  elseif ( z == 0 )
    s = sum ( bvec(1:n) );
    if ( s == n )
      bvec(1:n) = 0;
    else
      bvec(1:n-s-1) = 0;
      bvec(n-s:n) = 1;
    end
%
%  Found the rightmost "01" string.
%  Replace it by "10", and shift following 1's to end.
%
  else
    bvec(z) = 1;
    bvec(o) = 0;
    s = sum ( bvec(o+1:n) );
    bvec(o+1:n-s) = 0;
    bvec(n+1-s:n) = 1;
  end

  return
end


function [K, R, t] = decomposeP(P)
%VGG_KR_FROM_P Extract K, R from camera matrix.
%
%    [K,R,t] = VGG_KR_FROM_P(P [,noscale]) finds K, R, t such that P = K*R*[eye(3) -t].
%    It is det(R)==1.
%    K is scaled so that K(3,3)==1 and K(1,1)>0. Optional parameter noscale prevents this.
%
%    Works also generally for any P of size N-by-(N+1).
%    Works also for P of size N-by-N, then t is not computed.

% Author: Andrew Fitzgibbon <awf@robots.ox.ac.uk>
% Modified by werner.
% Date: 15 May 98

N = size(P,1);
H = P(:,1:N);

[K,R] = rq(H);

K = K*sign(K(1));
if prod(sign(diag(K))) < 0
  D = diag(-1*sign(diag(K)));
else
  D = diag(sign(diag(K)));
end
K = K*D;
R = D*R;

K = K/K(end);

% $$$ if nargin < 2
% $$$   K = K / K(N,N);
% $$$   if K(1,1) < 0
% $$$     D = diag([-1 -1 ones(1,N-2)]);
% $$$     K = K * D;
% $$$     R = D * R;
% $$$
% $$$   %  test = K*R;
% $$$   %  vgg_assert0(test/test(1,1) - H/H(1,1), 1e-07)
% $$$   end
% $$$ end

if nargout > 2
  t = -P(:,1:N)\P(:,end);
end

return


function Anew = h_trid(A)
%  H_TRID(A) uses Householder method to form a tridiagonal matrix from A.
%  Must have a SQUARE SYMMETRIC matrix as the input.
%
%
%  Example:
%
%             B=[0 1 1;1 2 1;1 1 1];
%             h_trid(B)
%
%
% Author:  Matt Fig
% Contact: popkenai@yahoo.com

[M N] = size(A);
if M~=N || ~isequal(A,A')  %This just screens matricies that can't work.
   error('Matrix must be square symmetric only, see help.');
end

lngth = length(A);  % Preallocations.
v = zeros(lngth,1);
I = eye(lngth);
Aold = A;

for jj=1:lngth-2  % Build each vector j and run the whole procedure.
    v(1:jj) = 0;
    S = ss(Aold,jj);
    v(jj+1) = sqrt(.5*(1+abs(Aold(jj+1,jj))/(S+2*eps)));
    v(jj+2:lngth) = Aold(jj+2:lngth,jj)*sign(Aold(jj+1,jj))...
                   /(2*v(jj+1)*S+2*eps);
    P = I-2*v*v';
    Anew = P*Aold*P;
    Aold = Anew;
end

Anew(abs(Anew(:))<5e-14)=0; % Tolerence.

function anss = ss(A,jj)
% Subfunction for h_trid.
anss = sqrt(sum(A(jj+1:end,jj).^2));


function J=SimFisherMatrix(obj,Prot,x,variables,sigma)
% Alexander, D.C., 2008. A general framework for experiment design in diffusion MRI and its application in measuring direct tissue-microstructure features. Magn. Reson. Med. 60, 439?448.
obj.Prot.(obj.MRIinputs{1}).Mat = Prot;
if ~exist('variables','var'), variables=1:5; end
if ~exist('sigma','var'), sigma=0.1; end

J=zeros(max(variables));
if ismethod(obj,'equation_x')
    S_nominal = obj.equation_x(x);
else
    S_nominal = obj.equation(x);
end
% Gaussian Noise
for i=variables
    for j=variables
        Xi = x;
        Xistep = max(1e-10,Xi(i) / 100);
        Xi(i) = Xi(i) + Xistep;
        Xj = x;
        Xjstep = max(1e-10,Xj(j) / 100);
        Xj(j) = Xj(j) + Xjstep;
        if ismethod(obj,'equation_x')
            J(i,j) = sum( 1./sigma.^2 .* (S_nominal - obj.equation_x(Xi))/Xistep .* (S_nominal - obj.equation_x(Xj))/Xjstep);
        else
            J(i,j) = sum( 1./sigma.^2 .* (S_nominal - obj.equation(Xi))/Xistep .* (S_nominal - obj.equation(Xj))/Xjstep);
        end
    end
end
J=J(variables,variables);

% % Rician Noise
% for i=1:variables
%     for j=variables
%         Xi=x;
%         Xi(i)=Xi(i)+Xi(i)/100;
%         Xj=x;
%         Xj(j)=Xj(j)+Xj(j)/100;
%
%         A = scd_model_GPD_composite(x,Ax);
%
%         Z=zeros(size(scheme,1),1);
%         for k=1:size(scheme,1)
%             Z(k) = integral(@(a) A(k)^2*besseli(1,A(k)*a/sigma^2)^2*besseli(0,A(k)*a/sigma^2)^-2  *a/sigma^2.*besseli(0,A(k)*a/sigma^2).*exp(-(A(k)^2+a^2)/2*sigma^2)  ,0,inf);
%         end
%
%         dAi = (scd_model_GPD_composite(x,Ax)-scd_model_GPD_composite(Xi,Ax))/(x(i)/100);
%         dAj = (scd_model_GPD_composite(x,Ax)-scd_model_GPD_composite(Xj,Ax))/(x(j)/100);
%
%         J(i,j)= sum(1/sigma^4.*(dAi.*dAj).*(Z-A.^2));
%     end
% end
% J=J(variables,variables);
% if J<0, error('bug'); end

end


function [mi2] = m22mi2(m2)
% Convert area from square meters to square miles.
% Chad A. Greene 2012
mi2 = m2*3.861021585425E-7;


function [X,names]=elipsod
% [X,names]=elipsod defines ellipsoidal
% coordinates with 0 < lm < b,
% b < t < c, and c < e <infinity
syms e t lm real
names=[lm,t,e];
b=1; c=2*b; b2=b^2; c2=c^2; cb=c2-b2;
x=e*t*lm/(b*c);
y=sqrt((e^2-b2)*(t^2-b2)*...
    (b2-lm^2)/(b2*cb));
z=sqrt((e^2-c2)*(c2-t^2)*...
     (c2-lm^2)/(c2*cb));
X=[x;y;z];


function op = prox_l1linf( q )

%PROX_L1LINF    L1-LInf block norm: sum of L-inf norms of rows.
%    OP = PROX_L1LINF( q ) implements the nonsmooth function
%        OP(X) = q * sum_{i=1:m} norm(X(i,:),Inf)
%    where X is a m x n matrix.  If n = 1, this is equivalent
%    to PROX_L1
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    then it must be positive and real.
%    If Q is a vector, it must be m x 1, and in this case,
%    the weighted norm OP(X) = sum_{i} Q(i)*norm(X(i,:),Inf)
%    is calculated.

if nargin == 0,
	q = 1;
elseif ~isnumeric( q ) || ~isreal( q ) || any(q <= 0),
	error( 'Argument must be positive.' );
end
op = tfocs_prox( @(x)f(x,q), @(x,t)prox_f(x,t,q), 'vector' );
end % end of main function

function v = f(x,q)
    if numel(q) ~= 1 && size(q,1) ~= size(x,1)
        error('Weight must be a scalar or a column vector');
    end
    v = sum( q.* max(abs(x),[],2) );
end

function x = prox_f(x,t,q)
  if nargin < 2,
      error( 'Not enough arguments.' );
  end

  [n,d] = size(x);
  dim   = 2;

  % Option 1: explicitly call prox_linf on the rows:
  % slow, but the chief benefit is that it is low memory
  % This would probably be faster if the matrix was transposed before and after
%   for k= 1:n
%       if isscalar(q), qk = q;
%       else, qk = q(k);
%       end
%       x(k,:) = prox_linf_q( qk, x(k,:).', t ).';
%   end
%   return;

% Option 2: vectorize the call.  By far, more efficient than option 1

  %s     = sort( abs(x), dim, 'descend' );
  %cs    = cumsum(s,dim);
  % Since Matlab stores matrices in column-major order, this method is more cache friendly:
  s     = sort( abs(x)', q, 'descend' );
  cs    = cumsum(s,1)';
  s     = s';

  s     = [s(:,2:end), zeros(n,1)];

  ndx1 = zeros(n,1);
  ndx2 = zeros(n,1);

  if isscalar(q),
      tq = repmat( t*q, n, d );
  else
      tq = repmat( t*q, 1, d );
  end
  %Z = cs - s*diag(1:d);
  % The above may require a lot of memory, so use spdiag or this:
  Z = cs - bsxfun(@times,s,1:d);

  Z = ( Z >= tq );
  Z = Z.';
  % now Z is d x n (typically d is large, n is small)

  % Not sure how to vectorize the find.
  % One option is to use the [i,j] = find(...) form,
  % but that's also extra work, since we can't just find the "first".
  if n > 5
      % avoid the for-loop
      ndx1  = (d+1)-sum(Z)'; % this is the first row with Z > 0
      ndx2  = ndx1;
      ndx2( ndx2 == d+1 ) = Inf;
      ndx1( ndx1 == d+1)  = d; % arbitrary, but do this so we don't have a special case later
  else
      % this might be slightly less memory, so keep the code
      for k = 1:n
          % This is why we transposed Z: due to column-major order,
          %     find( columnVector ) is faster than find( rowVector )
          ndxk = find( Z(:,k), 1 );
          if ~isempty(ndxk)
              ndx1(k) = ndxk;
              ndx2(k) = ndxk;
          else
              ndx1(k) = 1; % value doesn't matter
              ndx2(k) = Inf;
          end
      end
  end
  indx_cs = sub2ind( [n,d], (1:n)', ndx1 );
  tau = (cs(indx_cs) - tq(:,1))./ndx2;
  tau = repmat( tau, 1, d );
  tau_noZeros = tau;
  tau_noZeros( ~x ) = 1;
  x   = x .* (  tau ./ max( abs(x), tau_noZeros ) );
  % Another, but not really better, way is to not do the rempat stuff and do:
  % x     = sign(x).*bsxfun( @min, tau, abs(x) );

end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function hermite_polynomial_test09 ( p, e )

%*****************************************************************************80
%
%% HERMITE_POLYNOMIAL_TEST09 tests HN_POWER_PRODUCT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer P, the maximum degree of the polynomial factors.
%
%    Input, integer E, the exponent of X in the integrand.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HERMITE_POLYNOMIAL_TEST09\n' );
  fprintf ( 1, '  Compute a normalized physicist''s Hermite polynomial power product table.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Tij = integral ( -oo < X < +oo ) X^E Hn(I,X) Hn(J,X) exp(-X*X) dx\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  where Hn(I,X) = normalized physicist''s Hermite polynomial of degree I.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Maximum degree P = %d\n', p );
  fprintf ( 1, '  Exponent of X = %d\n', e );

  table = hn_power_product ( p, e );

  r8mat_print ( p + 1, p + 1, table, '  Power product table:' );

  return
end


function [dist1, dist2] = distance2(i, bgevent, ac, newcat)
    % calculates the distance in [km] between two eqs
    % precise version based on Raesenbergs Program
    % the calculation is done simultaniously for the biggest event in the
    % cluster and for the current event
    %
    % A. Allmann

    global err derr

    if isempty(err)
        err=1.5; % from InitVariables --epicenter error
    end

    if isempty(derr)
        derr=2; % from InitVariables --depth error
    end

    % columns
    LAT = 2;
    LON = 1;
    DEP = 3;

    % if any of these fail, then we are dealing with multiple events simultaneously, and must take that into account
    assert(numel(i)==1)
    assert(numel(bgevent) == 1)

    evs = newcat.XYZ([i,bgevent,reshape(ac,1,[])],:);

    evs(:,[LAT,LON]) = deg2rad(evs(:,[LAT,LON])); % convert lats & lons to radians

    i_ev = evs(1,:);
    big_ev = evs(2,:);
    ac_ev = evs(3,:);

    pi2 = pi/2;
    flat= 0.993231;

    tana = flat .* tan([i_ev(LAT); big_ev(LAT)]);
    acol = pi2 - atan(tana);

    tanb = flat * tan(ac_ev(LAT));
    bcol = pi2 - atan(tanb);

    diflon = (ac_ev(LON) - [i_ev(LON),  big_ev(LON)]);

    cosdel = [ ...
        (sin(acol(1))*sin(bcol)) .* cos(diflon(1)) + (cos(acol(1))*cos(bcol)) , ...
        (sin(acol(2))*sin(bcol)) .* cos(diflon(2)) + (cos(acol(2))*cos(bcol)) ];

    colat = pi2 - (ac_ev(LAT) + [i_ev(LAT), big_ev(LAT)] ./2);

    radius = 6371.227*(1+(3.37853e-3)*(1/3-((cos(colat)).^2)));
    r = acos(cosdel) .* radius;            % epicenter distance

    r = r - 1.5 * err;               %influence of epicenter error

    r(r<0) = 0;

    % depth distance
    z = abs(ac_ev(DEP) - [i_ev(DEP), big_ev(DEP)] );
    z = z - derr;
    z(z<0)=0;

    r = sqrt(z .^ 2 + r.^2);                   %hypocenter distance
    dist1 = r(:,1);           %distance between eqs
    dist2 = r(:,2);


%SAMMONM Sammon mapping
%
%   W = SAMMONM(A,K,MAX)
%   W = A*SAMMONM([],K,MAX)
%   W = A*SAMMONM(K,MAX)
%   D = B*W
%
% INPUT
%   A    Dataset, used for training the mapping
%   B    Dataset, same dimensionality as A, to be mapped
%   K    Target dimension of mapping (default 2)
%   MAX  Maximum number of iterations, default 100
%
% OUTPUT
%   W    Trained mapping
%   D    K-dimensional dataset
%
% DESCRIPTION
% This is a simplified interface to the more complex MDS routine for high
% dimensional data visualisation. The output is a non-linear projection of
% the original vector space to a K-dimensional target space.
%
% The main differences with MDS are that SAMMONM operates on feature based
% datasets, while MDS expects dissimilarity matrices; MDS maps new objects
% by a second optimization procedures minimizing the stress for the test
% objects, while SAMMONM uses a linear mapping between dissimilarities and
% the target space. See also PREX_MDS for examples. A different procedure
% for the same purpose is TSNEM.
%
% EXAMPLE
% prdatasets;            % make sure prdatasets is in the path
% a = satellite;         % 36D dataset, 6 classes, 6435 objects
% [x,y] = gendat(a,0.5); % split in train and test set
% w = x*sammonm;         % compute mapping
% figure; scattern(x*w); % show trainset mapped to 2D: somewhat overtrained
% figure; scattern((x+randn(size(x))*1e-5)*w): % some noise helps
% figure; scattern(y*w); % show test set mapped to 2D
%
% REFERENCES
% 1. JW Sammon Jr A nonlinear mapping for data structure analysis,
%   IEEE Transactions on Computers C-18, pp. 401-409,1969.
% 2. E. Pekalska, D. de Ridder, R.P.W. Duin, and M.A. Kraaijveld, A new
%    method of generalizing Sammon mapping with application to algorithm
%    speed-up, ASCI99, Proc. 5th Annual ASCI Conf., 1999, 221-228. [pdf]
%
% SEE ALSO (PRTools Guide)
% DATASETS, MAPPINGS, PCAM, MDS, TSNEM, PREX_MDS, SCATTERD, SCATTERN

% Copyright: E. Pekalska, R.P.W. Duin, r.p.w.duin@37steps.com

function out = sammonm(varargin)

  argin = shiftargin(varargin,'scalar');
  argin = setdefaults(argin,[],2,100,0.01);
  if mapping_task(argin,'definition')
    out = define_mapping(argin,'untrained');
  elseif mapping_task(argin,'training')
    [a,k,max_iter,pow] = deal(argin{:});
    d = sqrt(distm(+a));
    opt.maxiter = max_iter;
    w = mds(d,a*pcam(a,k),opt);
    y = getdata(w,1);
    % refer to distances with a low power to get some smoothness
    v = prpinv(d.^pow)*y;
    out = trained_mapping(a,{v,a,pow},2);
  elseif mapping_task(argin,'trained execution')
    [b,w] = deal(argin{1:2});
    [v,a,pow] = getdata(w);
    out = setdata(b,(sqrt(distm(b,a)).^pow)*v);
  else
    error('Illegal call');
  end

return


% T in K
% es in Pa
function es = calculate_saturation_vapor_pressure_liquid (T, method)
    if (nargin < 2) || isempty(method),  method = 'Murphy&Koop2005';  end
    switch method
    case {'Bolton1980', 'approx'}
        t = T - 273.15;
        es2 = 0.6112 .* exp(17.67 .* t ./ (t + 243.5));
        es = es2 * 1000;
        % The AMS Glossary gives [1] the simplified formula above,
        % accordingly to Bolton (1980, eq. 10).
        %
        % AMS Glossary. <http://amsglossary.allenpress.com/glossary/search?id=clausius-clapeyron-equation1>
        % Bolton, D., 1980: The computation of equivalent potential temperature. Mon. Wea. Rev., 108, 1046-1053. <http://dx.doi.org/10.1175/1520-0493(1980)108<1046:TCOEPT>2.0.CO;2>
    case 'Murphy&Koop2005'
        if ~all(123 < T & T < 332)
            warning('calculate_saturation_vapor_pressure_liquid:outRange', ...
                'Temperature out of range [123-332] K.');
        end
        temp = 54.842763 - 6763.22 ./ T - 4.210 .* log(T) + 0.000367 .* T ...
          + tanh( 0.0415 * (T - 218.8) ) ...
          .* (53.878 - 1331.22 ./ T - 9.44523 .* log(T) + 0.014025 .* T);
        es = exp(temp);
        % D. M. MURPHY and T. KOOP
        % Review of the vapour pressures of ice and supercooled water for
        % atmospheric applications
        % Q. J. R. Meteorol. Soc. (2005), 131, pp. 1539-1565
        % doi: 10.1256/qj.04.94
        % <http://dx.doi.org/10.1256/qj.04.94>
        %
        % "Widely used expressions for water vapour (Goff and Gratch 1946; Hyland and Wexler 1983) are being applied outside the ranges of data used by the original authors for their fits. This work may be the first time that data on the molar heat capacity of supercooled water [i.e., at temperatures below its freezing temperature] have been used to constrain its vapour pressure.
    otherwise
        error('calculate_saturation_vapor_pressure_liquid:methodUnknown', ...
            'Method "%s" unknown.', method);
    end
end

%!test
%! % Table C1. VALUES RECOMMENDED FOR CHECKING COMPUTER CODES
%! temp = [...
%! % Temperature (K), Liquid vapor pressure (Pa)
%! 150     1.562e-5    0.001e-5
%! 180     0.011239    0.000001
%! 210     1.2335      0.0001
%! 240     37.667      00.001
%! 273.15  611.213     000.001
%! 273.16  611.657     000.001
%! 300     3536.8      0000.1
%! ];
%! T = temp(:,1);
%! es = temp(:,2);
%! tol = temp(:,3)/10*5;  % rounding error.
%! es2 = calculate_saturation_vapor_pressure_liquid (T);
%! %[es, es2, es2-es]  % DEBUG
%! myassert(es2, es, -tol);

%!test
%! T = (123:10:332)';
%! es  = calculate_saturation_vapor_pressure_liquid (T, 'Murphy&Koop2005');
%! es2 = calculate_saturation_vapor_pressure_liquid (T, 'Bolton1980');
%! %[T-273.15, es, es2, es2-es, 100*(es2-es)./es]  % DEBUG
%! % worse agreement (in percentage) for supercooled temperatures, as expected.
%! temp = 100*(es2-es)./es;
%! myassert(abs(temp(1)) > abs(temp(end)))


function img = discreteHalfPlane(varargin)
%DISCRETEHALFPLANE Discretize a half plane
%
%   A Halfplane is the set of point delimited by a straight line.
%   Only the points located 'on the left' of the line belong to the
%   halfplane.
%
%   IMG = discreteHalfPlane(DIM, LINE)
%   DIM is the size of image, with the format [x0 dx x1;y0 dy y1]
%   LINE is a 1x4 array of the form [x0 y0 dx dy], x0 and y0 being
%   coordinate of a point belonging to the boundary line, and dx and dy
%   being direction vectors of the boundary line of the halfplane.
%
%   IMG = discreteHalfPlane(DIM, POINT, DIRECTION)
%   POINT is a point belonging to the boundary line
%   DIRECTION is a 2x1 vector containing direction vector of the boundary
%   line.
%
%   IMG = discreteHalfPlane(LX, LY, ...);
%   Specifes the pixels coordinates with the two row vectors LX and LY.
%
%   Example
%   img = discreteHalfPlane([1 1 100;1 1 100], [50 50], 30, 10);
%
%   See Also
%     imShapes, discreteDisc, discreteSquare
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2006-10-12
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY
%   04/01/2007: concatenate transforms before applying them
%   04/03/2009: use meshgrid
%   29/04/2009: update transforms
%   29/05/2009: use more possibilities for specifying grid
%   22/01/2010: fix auto center with odd image size

% compute coordinate of image voxels
[lx, ly, varargin] = parseGridArgs(varargin{:});
[x, y]   = meshgrid(lx, ly);

% default parameters
center = [lx(ceil(end/2)) ly(ceil(end/2))];
theta   = 0;

% process input parameters
if length(varargin)==1
    % first argument contains all parameters
    var = varargin{1};
    center = var(1:2);
    theta = atan2(var(4), var(3));

elseif ~isempty(varargin)
    % parameters are given as separate arguments
    center = varargin{1};
    var = varargin{2};
    theta = atan2(var(2), var(1));
end

% transforms voxels according to square orientation
tra     = createTranslation(-center);
rot     = createRotation(-theta);
[x, y]  = transformPoint(x, y, rot*tra); %#ok<ASGLU>

% create image : simple threshold over 1 dimension
img = y > -1e-14;


function gegenbauer_poly_test ( )

%*****************************************************************************80
%
%% GEGENBAUER_POLY_TEST tests GEGENBAUER_POLY.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GEGENBAUER_POLY_TEST:\n' );
  fprintf ( 1, '  GEGENBAUER_POLY computes values of \n' );
  fprintf ( 1, '    the Gegenbauer polynomial.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       N       A       X       GPV      GEGENBAUER\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, a, x, fx ] = gegenbauer_poly_values ( n_data );

    if ( n_data == 0 )
      break
    end

    c = gegenbauer_poly ( n, a, x );
    fx2 = c(n+1);

    fprintf ( 1, '  %6d  %8f  %8f  %12f  %12f\n', n, a, x, fx, fx2 );

  end

  return
end


function [res,K] = dotk(x,y,n)
%DOTK Dot product in K-fold precision.
%   dotk(x,y) computes the scalar product x'*y of the vectors x and y as in
%   K-fold precision, where K is chosen adaptively so that the result is
%   accurate up to machine precision. If x and y are N-D arrays, dotk(x,y)
%   operates along their first non-singleton dimension.
%
%   dotk(x,y,n) returns the dot product of x and y in the dimension n.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] T. Ogita, S. M. Rump, S. Oishi, "Accurate sum and dot product",
%       SIAM J. Sci. Comp., Vol. 26, No. 6, 2005, pp. 1955-1988.

% Permute x and y so that the n-th mode is the first one.
sx = size(x);
sy = size(y);
if any(sx ~= sy), error('dotk:xy','x and y should have equal size.'); end
if nargin < 3, n = find(sx > 1,1); end
if isempty(n) || size(x,n) == 1, res = conj(x).*y; K = 0; return; end

% Call recursively if either x or y is complex.
if ~isreal(x) && isreal(y)
    [resr,Kr] = dotk(real(x),y,n);
    [resi,Ki] = dotk(-imag(x),y,n);
    res = resr+resi*1i;
    K = Kr+Ki*1i;
    return;
elseif isreal(x) && ~isreal(y)
    [resr,Kr] = dotk(x,real(y),n);
    [resi,Ki] = dotk(x,imag(y),n);
    res = resr+resi*1i;
    K = Kr+Ki*1i;
    return;
elseif ~isreal(x) && ~isreal(y)
    x = cat(n,real(x),imag(x));
    [resr,Kr] = dotk(x,cat(n,real(y),imag(y)),n);
    [resi,Ki] = dotk(x,cat(n,imag(y),-real(y)),n);
    res = resr+resi*1i;
    K = Kr+Ki*1i;
    return;
end
if n > 1
    x = permute(x,[n 1:n-1 n+1:ndims(x)]);
    y = permute(y,[n 1:n-1 n+1:ndims(y)]);
end

% Initialize the algorithm.
res = zeros([2*sx(n) sx(1:n-1) sx(n+1:length(sx))]);
factor = 2^ceil(log2(2/eps(class(x)))/2)+1;

% Adaptive K-fold precision dot product.
[res(end,:),res(1,:)] = twoproduct(x(1,:),y(1,:),factor);
for i = 2:size(x,1)
    [tmp,res(i,:)] = twoproduct(x(i,:),y(i,:),factor);
    [res(end,:),res(i+size(x,1)-1,:)] = twosum(res(end,:),tmp);
end
[res,K] = sumk(res);
if n > 1, res = permute(res,[2:n 1 n+1:length(sx)]); end
if isreal(K), K = K+1; else K = K+1+1*1i; end

end

function [x,y] = twoproduct(a,b,factor)

tmp = factor*a;
a1 = tmp-(tmp-a);
a2 = a-a1;
tmp = factor*b;
b1 = tmp-(tmp-b);
b2 = b-b1;

x = a.*b;
y = a2.*b2-(((x-a1.*b1)-a2.*b1)-a1.*b2);

end

function [x,y] = twosum(a,b)

x = a+b;
tmp = x-a;
y = (a-(x-tmp))+(b-tmp);

end


function img = discreteSquare(varargin)
%DISCRETESQUARE Discretize a planar square
%
%   IMG = discreteSquare(DIM, CENTER, SIDE)
%   DIM is the size of image, with the format [x0 dx x1;y0 dy y1]
%   CENTER is the center of the square
%   SIDE is the length of the side of the square.
%
%   IMG = discreteSquare(DIM, CENTER, SIDE, THETA)
%   Also specify spherical angle of the normal of a face of the square.
%   THETA is the angle with the horizontal, in degrees, counted counter-
%   clockwise in direct basis (and clockwise in image basis).
%
%   IMG = discreteSquare(DIM, SQUARE)
%   send parameters in a row vector, where SQUARE contains at least the
%   center coordinate, and possibly the other parameters.
%
%   IMG = discreteSquare(LX, LY, ...);
%   Specifes the pixels coordinates with the two row vectors LX and LY.
%
%   Example
%   img = discreteSquare([1 1 100;1 1 100], [50 50], 30);
%   img = discreteSquare([1 1 100;1 1 100], [50 50 30]);
%   img = discreteSquare([1 1 100;1 1 100], [50 50 30 30]);
%
%   See Also
%   imShapes, discreteDisc, discreteRectangle
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2006-05-16
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY
%   12/10/2006: typo in the doc
%   04/01/2007: concatenate transforms before applying them
%   19/06/2007: update doc
%   04/03/2009: use meshgrid
%   29/04/2009: update transforms
%   29/05/2009: use more possibilities for specifying grid
%   22/01/2010: fix auto center with odd image size
%   2011-03-30 use degrees

% compute coordinate of image voxels
[lx, ly, varargin] = parseGridArgs(varargin{:});
[x, y]   = meshgrid(lx, ly);

% default parameters
center = [lx(ceil(end/2)) ly(ceil(end/2))];
side    = center;
theta   = 0;

% process input parameters
if length(varargin)==1
    % all paramater in the first argument
    var = varargin{1};
    center = var(:,1:2);
    if size(var, 2)>2
        side = var(:,3);
    end
    if size(var, 2)>3
        theta = var(:,4);
    end

elseif ~isempty(varargin)
    % first argument is center, optionnally followed by side length, then
    % by rotation angle
    center = varargin{1};
    if length(varargin)>1
        side = varargin{2};
    end
    if length(varargin)>2
        theta = varargin{3};
    end
end

% For squares, size of sides is the same in all directions
side    = [side(1) side(1)];

% transforms voxels according to square orientation
tra     = createTranslation(-center);
rot     = createRotation(-deg2rad(theta));
sca     = createScaling(1./side);
[x, y]  = transformPoint(x, y, sca*rot*tra);

% create image: simple threshold over 2 dimensions
img = abs(x) <= .5 & abs(y) <= .5;


function y = GUSFT_simple(x,shift,boxlen,center,w);
% GUSFT_simple -- Gram matrix of the USFT
% Usage:
%   y = GUSFT_simple(x,shift,boxlen,center,w)
% Inputs:
%   x	    vector of length n
%   shift   vector of shifts
%   boxlen  half-length of the window associated with shift
%   center  boolean variable
%   w       window of size 2*boxlen
% Outputs:
%   y      vector le length 2*n
% Description:
%  Evaluates the A'A where A is the nonuniform FT at the points omega_k =
%  shift + k, -boxlen <= k <boxlen
%  See Also
%    USFT_simple, USFFT, MakeFourierDiagonal, GUSFT_Toeplitz
%
% By Emmanuel candes, 2003-2004

if nargin < 5,
  w = ones(1,2*boxlen);
end

if nargin < 4,
  center = 0;
end

       tx  =  USFT_simple(x,shift,boxlen,center,w);
       y   =  Adj_USFT_simple(tx,shift,boxlen,center,w);


function corrected = apply_cmatrix(im,cmatrix)
% CORRECTED = apply_cmatrix(IM,CMATRIX)
%
% Applies CMATRIX to RGB input IM. Finds the appropriate weighting of the
% old color planes to form the new color planes, equivalent to but much
% more efficient than applying a matrix transformation to each pixel.
if size(im,3)~=3
error('Apply cmatrix to RGB image only.')
end
r = cmatrix(1,1)*im(:,:,1)+cmatrix(1,2)*im(:,:,2)+cmatrix(1,3)*im(:,:,3);
g = cmatrix(2,1)*im(:,:,1)+cmatrix(2,2)*im(:,:,2)+cmatrix(2,3)*im(:,:,3);
b = cmatrix(3,1)*im(:,:,1)+cmatrix(3,2)*im(:,:,2)+cmatrix(3,3)*im(:,:,3);
corrected = cat(3,r,g,b);
end


function Sk = mriphantom(kx,ky)
%========================================================================
% Sk = function mriphantom(kx,ky)
%
% Creates simulated raw MRI data of a Shepp Logan head phantom
% for given k space points
% input is the kx and ky coordinates in the image frequency space
% default values for kx and ky are for a Carthesian sampling.
% Input
%   kx, ky  Matrices with kx and ky sampling coordinates
%   E       Optional analytical phantom data matrix as produced by
%           image toolbox function phantom.m (see comments below).
% Data are from an analytical expression for a continuous phantom.
% Core equations for this are taken form :
%  Rik van der Walle et al IEEE Trans Med Imag 19(12) p1160.
%
% Note that the standard matlab function just gives the phantom
% sampled in image space on a carthesian grid.
% This function delivers the simulated raw MRI data sampled through
% a user defined path in k-space (image frequency space)
%
% Author : Ronald Ouwekerk Johns Hopkins University Dept. Radiology
% 601 N. Caroline Street Room 4250 Baltimore, MD 21287-0845 USA
% rouwerke@mri.jhu.edu
% Written :May 2002
% Modified June 2002 :  Revised to give a non-NaN result for k=0
%                       Suppress output if nargout == 0
%                       Added comments to explain the difference with phantom.m
%
% Whenever this software is used for publications an acknowledgment
% would be much appreciated.
%                           RO
%========================================================================
FOV = 2;
N = 64;

if nargin < 2
    %=====================================================================
    % Set the field of view to 24 cm (see image toolbox phantom.m)
    % resolution 128x128 points
    % create k-space coordinates for Carthesion sampling
    % Should be equivalent to IFFT of the phantom
    %=====================================================================
    FOV = 0.24;
    res = FOV/N;
    Kmax = 1/(2*res);
    ky = linspace(-Kmax,Kmax, N);
    ky = ky(ones(N,1),:);
    kx = permute(ky, [2,1]);
end;
if ( (nargin < 3 ) | isempty(E) )  & (exist('phantom.m')==2)
    %========================================================================
    % Create 2D phantom to get defining matrix E
    %========================================================================
    [P,E] = phantom;
elseif ~(exist('phantom.m')==2)
    P = [];
    E = modified_shepp_logan;
end;

%========================================================================
% copy the elements of E and scale to field of view
% Matlab phantom.m default yields FOV =[-1,1]= 2
% Scale all dimensions to desired FOV (*FOV/2).
%   Column 1:  rho  the additive intensity value of the ellipse
%   Column 2:  a    the length of the horizontal semi-axis of the ellipse
%   Column 3:  b    the length of the vertical semi-axis of the ellipse
%   Column 4:  x0   the x-coordinate of the center of the ellipse
%   Column 5:  y0   the y-coordinate of the center of the ellipse
%   Column 6:  alpha  the angle (in degrees) between the horizontal semi-axis
%                   of the ellipse and the x-axis of the image
%========================================================================
[me,ne] = size(E);
rho = E(:,1)';
nellipses = length(rho);
a   = FOV/2 * E(:,2)';
b   = FOV/2 * E(:,3)';
x0  = FOV/2 * E(:,4)';
y0  = FOV/2 * E(:,5)';
alpha = pi*E(:,6)'/180;

%========================================================================
% Stretch kspace coordinates to column vector (retain original size info)
%========================================================================
[mk,nk] = size(kx);
kx = kx(:);
ky = ky(:);
k = kx + j*ky;
klen = length(kx);

%========================================================================
% Calculate angle of the vector to the ellipse center in real space.
%========================================================================
gamma = atan2(y0, x0);
te = sqrt(x0.^2 + y0.^2);

%========================================================================
% Replicate all the ellipse defining vectors to the number of k space values
%========================================================================
gamma = gamma(ones(klen,1),:);
te = te(ones(klen,1),:);
alpha = alpha(ones(klen,1),:);
rho = rho(ones(klen,1),:);
a = a(ones(klen,1),:);
b = b(ones(klen,1),:);

%========================================================================
% Calculate k space vectors angle and magnitude
%========================================================================
kphi = angle(k);
kr = abs(k);

%========================================================================
% Replicate k space and time vectors to the number of ellipses
%========================================================================
kr = kr(:,ones(1,nellipses));
kphi = kphi(:,ones(1,nellipses));

%========================================================================
% Precalculate some values
%========================================================================
pi2 = 2*pi;
cose = cos(gamma - kphi);

%========================================================================
% Projection angles:
% Difference of the k space vector angle and the angles of the ellipses
%========================================================================
sind = sin(kphi - alpha);
cosd = cos(kphi - alpha);

akphi = sqrt(a.^2 .* cosd.^2 + b.^2 .* sind.^2);
%========================================================================
% Calculate the signals for each ellips and each k space point kx,ky
% avoid division by zero    besselj(1,0) = 0 so set 0/0 = 1
%========================================================================
knz = (akphi.*kr ~=0);
bess = ones(size(akphi.*kr));
bess(knz) = besselj(1,pi2.*akphi(knz).*kr(knz)) ./(akphi(knz).*kr(knz));
Sk = exp(-j*pi2.*kr.*te.*cose) .*rho.*a.*b.* bess;
%========================================================================
% Sum the signals for all ellipses and reshape the data back to
% the matrix size of the kx and ky input data
%========================================================================
Sk = sum(Sk,2);
Sk = reshape(Sk, mk,nk);
%========================================================================
% Show the result, including the reconstruction if default Carthesian
% sampling was used
%========================================================================
if  nargin >= 2
    fh = figure;
    subplot(1,2,1);
    if ~isempty(P)
        imagesc(abs(P));
    end;
    title('Software head phantom')
    axis image

    subplot(1,2,2);
    imagesc(abs(Sk));
    title('Simulated raw data')
    return
else
    fh = figure;
    subplot(1,3,1);
    if ~isempty(P)
        imagesc(abs(P));
    end;
    axis image

    subplot(1,3,2);
    imagesc(abs(Sk));
    title('Simulated raw data')
    axis image

    subplot(1,3,3);
    I = fftshift(fft2(Sk));
    imagesc(abs(I));
    axis image
    title('2DFFT Reconstructed image')
end;
%========================================================================
% Suppress output if not asked for any
%========================================================================
if nargout < 1
    Sk = [];
end;

%========================================================================
% END
%========================================================================

%========================================================================
% local function Copied from function phantom.m
% to produce a result without the image toolbox
%========================================================================

%========================================================================
function toft=modified_shepp_logan
%
%   This head phantom is the same as the Shepp-Logan except
%   the intensities are changed to yield higher contrast in
%   the image.  Taken from Toft, 199-200.
%
%         A    a     b    x0    y0    phi
%        ---------------------------------
toft = [  1   .69   .92    0     0     0
        -.8  .6624 .8740   0  -.0184   0
        -.2  .1100 .3100  .22    0    -18
        -.2  .1600 .4100 -.22    0     18
         .1  .2100 .2500   0    .35    0
         .1  .0460 .0460   0    .1     0
         .1  .0460 .0460   0   -.1     0
         .1  .0460 .0230 -.08  -.605   0
         .1  .0230 .0230   0   -.606   0
         .1  .0230 .0460  .06  -.605   0   ];
%========================================================================
% end local function toft=modified_shepp_logan
%========================================================================


function [ y, n ] = r8_upak ( x )

%*****************************************************************************80
%
%% R8_UPAK unpacks an R8 into a mantissa and exponent.
%
%  Discussion:
%
%    This function unpacks a floating point number x so that
%
%      x = y * 2.0^n
%
%    where
%
%      0.5 <= abs ( y ) < 1.0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2011
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, real X, the number to be unpacked.
%
%    Output, real Y, the mantissa.
%
%    Output, integer N, the exponent.
%
  absx = abs ( x );
  n = 0;
  y = 0.0;

  if ( x == 0.0 )
    return
  end

  while ( absx < 0.5 )
    n = n - 1;
    absx = absx * 2.0;
  end

  while ( 1.0 <= absx )
    n = n + 1;
    absx = absx * 0.5;
  end

  if ( x < 0.0 )
    y = - absx;
  else
    y = + absx;
  end

  return
end


#!/usr/bin/env octave
%% Machine Learning Online Class
%  Exercise 5 | Regularized Linear Regression and Bias-Variance
%
%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  exercise. You will need to complete the following functions:
%
%     linearRegCostFunction.m
%     learningCurve.m
%     validationCurve.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%

%% Initialization
clear ; close all; clc

%% =========== Part 1: Loading and Visualizing Data =============
%  We start the exercise by first loading and visualizing the dataset.
%  The following code will load the dataset into your environment and plot
%  the data.
%

% Load Training Data
fprintf('Loading and Visualizing Data ...\n')

% Load from ex5data1:
% You will have X, y, Xval, yval, Xtest, ytest in your environment
load ('ex5data1.mat');

% m = Number of examples
m = size(X, 1);

% Plot training data
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');

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

%% =========== Part 2: Regularized Linear Regression Cost =============
%  You should now implement the cost function for regularized linear
%  regression.
%

theta = [1 ; 1];
J = linearRegCostFunction([ones(m, 1) X], y, theta, 1);

fprintf(['Cost at theta = [1 ; 1]: %f '...
         '\n(this value should be about 303.993192)\n'], J);

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

%% =========== Part 3: Regularized Linear Regression Gradient =============
%  You should now implement the gradient for regularized linear
%  regression.
%

theta = [1 ; 1];
[J, grad] = linearRegCostFunction([ones(m, 1) X], y, theta, 1);

fprintf(['Gradient at theta = [1 ; 1]:  [%f; %f] '...
         '\n(this value should be about [-15.303016; 598.250744])\n'], ...
         grad(1), grad(2));

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

%% =========== Part 4: Train Linear Regression =============
%  Once you have implemented the cost and gradient correctly, the
%  trainLinearReg function will use your cost function to train
%  regularized linear regression.
%
%  Write Up Note: The data is non-linear, so this will not give a great
%                 fit.
%

%  Train linear regression with lambda = 0
lambda = 0;
[theta] = trainLinearReg([ones(m, 1) X], y, lambda);

%  Plot fit over the data
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');
hold on;
plot(X, [ones(m, 1) X]*theta, '--', 'LineWidth', 2)
hold off;

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

%% =========== Part 5: Learning Curve for Linear Regression =============
%  Next, you should implement the learningCurve function.
%
%  Write Up Note: Since the model is underfitting the data, we expect to
%                 see a graph with "high bias" -- slide 8 in ML-advice.pdf
%

lambda = 0;
[error_train, error_val] = ...
    learningCurve([ones(m, 1) X], y, ...
                  [ones(size(Xval, 1), 1) Xval], yval, ...
                  lambda);

plot(1:m, error_train, 1:m, error_val);
title('Learning curve for linear regression')
legend('Train', 'Cross Validation')
xlabel('Number of training examples')
ylabel('Error')
axis([0 13 0 150])

fprintf('# Training Examples\tTrain Error\tCross Validation Error\n');
for i = 1:m
    fprintf('  \t%d\t\t%f\t%f\n', i, error_train(i), error_val(i));
end

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

%% =========== Part 6: Feature Mapping for Polynomial Regression =============
%  One solution to this is to use polynomial regression. You should now
%  complete polyFeatures to map each example into its powers
%

p = 8;

% Map X onto Polynomial Features and Normalize
X_poly = polyFeatures(X, p);
[X_poly, mu, sigma] = featureNormalize(X_poly);  % Normalize
X_poly = [ones(m, 1), X_poly];                   % Add Ones

% Map X_poly_test and normalize (using mu and sigma)
X_poly_test = polyFeatures(Xtest, p);
X_poly_test = bsxfun(@minus, X_poly_test, mu);
X_poly_test = bsxfun(@rdivide, X_poly_test, sigma);
X_poly_test = [ones(size(X_poly_test, 1), 1), X_poly_test];         % Add Ones

% Map X_poly_val and normalize (using mu and sigma)
X_poly_val = polyFeatures(Xval, p);
X_poly_val = bsxfun(@minus, X_poly_val, mu);
X_poly_val = bsxfun(@rdivide, X_poly_val, sigma);
X_poly_val = [ones(size(X_poly_val, 1), 1), X_poly_val];           % Add Ones

fprintf('Normalized Training Example 1:\n');
fprintf('  %f  \n', X_poly(1, :));

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

%% =========== Part 7: Learning Curve for Polynomial Regression =============
%  Now, you will get to experiment with polynomial regression with multiple
%  values of lambda. The code below runs polynomial regression with
%  lambda = 0. You should try running the code with different values of
%  lambda to see how the fit and learning curve change.
%

lambda = 0;
[theta] = trainLinearReg(X_poly, y, lambda);

% Plot training data and fit
figure(1);
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
plotFit(min(X), max(X), mu, sigma, theta, p);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');
title (sprintf('Polynomial Regression Fit (lambda = %f)', lambda));

figure(2);
[error_train, error_val] = ...
    learningCurve(X_poly, y, X_poly_val, yval, lambda);
% Should we use logarithmix scale there?
plot(1:m, error_train, 1:m, error_val);

title(sprintf('Polynomial Regression Learning Curve (lambda = %f)', lambda));
xlabel('Number of training examples')
ylabel('Error')
axis([0 13 0 100])
legend('Train', 'Cross Validation')

fprintf('Polynomial Regression (lambda = %f)\n\n', lambda);
fprintf('# Training Examples\tTrain Error\tCross Validation Error\n');
for i = 1:m
    fprintf('  \t%d\t\t%f\t%f\n', i, error_train(i), error_val(i));
end

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

%% =========== Part 8: Validation for Selecting Lambda =============
%  You will now implement validationCurve to test various values of
%  lambda on a validation set. You will then use this to select the
%  "best" lambda value.
%

[lambda_vec, error_train, error_val] = ...
    validationCurve(X_poly, y, X_poly_val, yval);

close all;
plot(lambda_vec, error_train, lambda_vec, error_val);
legend('Train', 'Cross Validation');
xlabel('lambda');
ylabel('Error');

fprintf('lambda\t\tTrain Error\tValidation Error\n');
for i = 1:length(lambda_vec)
	fprintf(' %f\t%f\t%f\n', ...
            lambda_vec(i), error_train(i), error_val(i));
end

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


function E=TeagerEnergy(x,dim)
%%TEAGERENERGY Compute the Teager energy of a discrete signal as defined in
%              [1]. The Teager energy is a type of instantaneous "energy"
%              signal that produces a result related to the rate of change
%              of the amplitude and frequency of the original signal. It
%              has found use in a number of applications, primarily related
%              to acoustic processing, such as in the wavelet-based speech
%              enhancement of [2].
%
%INPUTS: x A vector signal or a matrix of signals whose Teager energy is
%          desired.
%      dim The dimension of x over which the Teager energy is to be found.
%          For example, for a NXnumSigs set of signals, one would use
%          dims=1 as each column is a separate signal.
%
%OUTPUTS: E The Teager energy. This has the same size as x.
%
%EXAMPLE 1:
%Here we compute the Teager energy signal of an up-chirp followeb by a down
%chirp.
% fStart=10;
% fEnd=100;
% T=1;
%
% %We want the chirp to be sampled at 100 times the Nyquist frequency.
% T0=1/(100*2*fEnd);%The sampling period.
% %We want to sample just the duration of the signal. Thus, time goes from
% %T=0 to T=1. This means that we need N=T/T0 samples.
% N=T/T0;
%
% %Make a chirp up and then a chirp down
% [x1,t]=LFMChirp(T,fStart,fEnd,T0);
% x2=LFMChirp(T,fEnd,fStart,T0);
% x=real([x1(:);x2(:)]);
% t=[t(:);t(end)+t(2)-t(1)+t(:)];
%
% E=TeagerEnergy(x);
%
% figure(1)
% clf
% subplot(2,1,1)
% hold on
% plot(t,x,'-r','linewidth',2)
% title('Original Signal')
% subplot(2,1,2)
% plot(t,E,'-r','linewidth',2)
% title('Transformed Signal')
%
%EXAMPLE 2:
%Here we compute the Teager energy of a sum of sinusoids. It essentially
%just follows the beat pattern.
% n=(0:100).';
% x=sin(pi/6*n)+sin(pi/4*n);
%
% E=TeagerEnergy(x);
%
% figure(2)
% clf
% subplot(2,1,1)
% hold on
% plot(n,x,'-r','linewidth',2)
% title('Original Signal')
% subplot(2,1,2)
% plot(n,E,'-r','linewidth',2)
% title('Transformed Signal')
%
%REFERENCES:
%[1] J. F. Kaiser, "On a simple algorithm to calculate the 'energy' of a
%    signal," in Proceedings of the International Conference on Acoustics,
%    Speech and Signal Processing, Albuquerque, NM, 3-6 Apr. 1990, pp.
%    381-384.
%[2] M. Bahoura and J. Rouat, "Wavelet speech enhancement based on the
%    Teager energy operator," IEEE Signal Processing Letters, vol. 8, no.
%    1, pp. 10-12, Jan. 2001.
%
%November 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(dim))
    E=x.^2-circshift(x,-1).*circshift(x,1);
else
    E=x.^2-circshift(x,-1,dim).*circshift(x,1,dim);
end
end

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


function hermite_exactness ( n, x, w, p_max )

%*****************************************************************************80
%
%% HERMITE_EXACTNESS investigates exactness of Hermite quadrature.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points in the rule.
%
%    Input, real X(N), the quadrature points.
%
%    Input, real W(N), the quadrature weights.
%
%    Input, integer P_MAX, the maximum exponent.
%    0 <= P_MAX.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Quadrature rule for Hermite integral\n' );
  fprintf ( 1, '  Rule of order N = %d\n', n );
  fprintf ( 1, '  Degree          Relative Error\n' );
  fprintf ( 1, '\n' );

  for p = 0 : p_max

    s = hermite_integral ( p );

    v(1:n,1) = x(1:n) .^ p;

    q = w' * v;

    if ( s == 0.0 )
      e = abs ( q - s );
    else
      e = abs ( ( q - s ) / s );
    end

    fprintf ( 1, '  %6d  %24.16f\n', p, e );

  end

  return
end


function K = covGaboriso(hyp, x, z, i)

% Gabor covariance function with length scale ell and period p. The
% covariance function is parameterized as:
%
% k(x,z) = h(x-z) with h(t) = exp(-t'*t/(2*ell^2))*cos(2*pi*sum(t)/p).
%
% The hyperparameters are:
%
% hyp = [ log(ell)
%         log(p)   ]
%
% Note that covSM implements a weighted sum of Gabor covariance functions, but
% using an alternative (spectral) parameterization.
%
% For more help on design of covariance functions, try "help covFunctions".
%
% Copyright (c) by Hannes Nickisch, 2014-09-26.
%
% See also COVFUNCTIONS.M, COVGABORARD.M, COVSM.M.

if nargin<2, K = '2'; return; end                          % report no of params
if nargin<3, z = []; end                                   % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

[n,D] = size(x);                                                % dimensionality
ell = exp(hyp(1));                                                % length scale
p = exp(hyp(2));                                                        % period

if dg                                              % compute squared distance d2
  d2 = zeros(n,1);
else
  if xeqz                                                 % symmetric matrix Kxx
    d2 = sq_dist(x'/ell);
  else                                                   % cross covariances Kxz
    d2 = sq_dist(x'/ell,z'/ell);
  end
end

dp = zeros(size(d2));                                % init sum(t)/p computation
if ~dg
  if xeqz                                                 % symmetric matrix Kxx
    for d=1:D, dp = dp + (x(:,d)*ones(1,size(x,1))-ones(n,1)*x(:,d)')/p; end
  else                                                   % cross covariances Kxz
    for d=1:D, dp = dp + (x(:,d)*ones(1,size(z,1))-ones(n,1)*z(:,d)')/p; end
  end
end

K = exp(-d2/2);
if nargin<4                                                        % covariances
  K = cos(2*pi*dp).*K;
else                                                               % derivatives
  if i==1                                                         % length scale
    K = d2.*cos(2*pi*dp).*K;
  elseif i==2                                                           % period
    K = 2*pi*dp.*sin(2*pi*dp).*K;
  else
    error('Unknown hyperparameter')
  end
end


% DFTFILT3 - discrete complex wavelet filters
%
% Usage:
%   >> [wavelet,cycles,freqresol,timeresol] = dftfilt3( freqs, cycles, srate, varargin)
%
% Inputs:
%   freqs    - vector of frequencies of interest.
%   cycles   - cycles array. If cycles=0, then the Hanning tapered Short-term FFT is used.
%              If one value is given and cycles>0, all wavelets have
%              the same number of cycles. If two values are given, the
%              two values are used for the number of cycles at the lowest
%              frequency and at the highest frequency, with linear or
%              log-linear interpolation between these values for intermediate
%              frequencies
%   srate    - sampling rate (in Hz)
%
% Optional Inputs: Input these as 'key/value pairs.
%   'cycleinc' - ['linear'|'log'] increase mode if [min max] cycles is
%              provided in 'cycle' parameter. {default: 'linear'}
%   'winsize'  Use this option for Hanning tapered FFT or if you prefer to set the length of the
%              wavelets to be equal for all of them (e.g., to set the
%              length to 256 samples input: 'winsize',256). {default: [])
%              Note: the output 'wavelet' will be a matrix and it may be
%              incompatible with current versions of timefreq and newtimef.
%   'timesupport' The number of temporal standard deviation used for wavelet lengths {default: 7)
%
% Output:
%   wavelet - cell array or matrix of wavelet filters
%   timeresol - temporal resolution of Morlet wavelets.
%   freqresol - frequency resolution of Morlet wavelets.
%
% Note: The length of the window is always made odd.
%
% Authors: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 3/28/2003
%          Rey Ramirez, SCCN/INC/UCSD, La Jolla, 9/26/2006

% Copyright (C) 3/28/2003 Arnaud Delorme 8, SCCN/INC/UCSD, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

%
% Revision 1.12 2006/09/25  rey r
% Almost complete rewriting of dftfilt2.m, changing both Morlet and Hanning
% DFT to be more in line with conventional implementations.
%
% Revision 1.11  2006/09/07 19:05:34  scott
% further clarified the Morlet/Hanning distinction -sm
%
% Revision 1.10  2006/09/07 18:55:15  scott
% clarified window types in help msg -sm
%
% Revision 1.9  2006/05/05 16:17:36  arno
% implementing cycle array
%
% Revision 1.8  2004/03/04 19:31:03  arno
% email
%
% Revision 1.7  2004/02/25 01:45:55  arno
% sinus test
%
% Revision 1.6  2004/02/15 22:23:08  arno
% implementing morlet wavelet
%
% Revision 1.5  2003/05/09 20:55:10  arno
% adding hanning function
%
% Revision 1.4  2003/04/29 16:02:54  arno
% header typos
%
% Revision 1.3  2003/04/29 01:09:16  arno
% debug imaginary part
%
% Revision 1.2  2003/04/28 23:01:13  arno
% *** empty log message ***
%
% Revision 1.1  2003/04/28 22:46:49  arno
% Initial revision
%

function [wavelet,cycles,freqresol,timeresol] = dftfilt3( freqs, cycles, srate, varargin);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Rey fixed all input parameter sorting.
if nargin < 3
    error(' A minimum of 3 arguments is required');
end
numargin=length(varargin);
if rem(numargin,2)
    error('There is an uneven number key/value inputs. You are probably missing a keyword or its value.')
end
varargin(1:2:end)=lower(varargin(1:2:end));

% Setting default parameter values.
cycleinc='linear';
winsize=[];
timesupport=7;  % Setting default of 7 temporal standard deviations for wavelet's length.

for n=1:2:numargin
    keyword=varargin{n};
    if strcmpi('cycleinc',keyword)
        cycleinc=varargin{n+1};
    elseif strcmpi('winsize',keyword)
        winsize=varargin{n+1};
        if ~mod(winsize,2)
            winsize=winsize+1; % Always set to odd length wavelets and hanning windows;
        end
    elseif strcmpi('timesupport',keyword)
        timesupport=varargin{n+1};
    else
        error(['What is ' keyword '? The only legal keywords are: type, cycleinc, winsize, or timesupport.'])
    end
end
if isempty(winsize) && cycles(1)==0
    error('If you are using a Hanning tapered FFT, please supply the winsize input-pair.')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% compute number of cycles at each frequency
% ------------------------------------------
type='morlet';
if length(cycles) == 1 && cycles(1)~=0
    cycles = cycles*ones(size(freqs));
elseif length(cycles) == 2
    if strcmpi(cycleinc, 'log') % cycleinc
         cycles = linspace(log(cycles(1)), log(cycles(2)), length(freqs));
         cycles = exp(cycles);
         %cycles=logspace(log10(cycles(1)),log10(cycles(2)),length(freqs)); %rey
    else
        cycles = linspace(cycles(1), cycles(2), length(freqs));
    end
end
if cycles==0
    type='sinus';
end

sp=1/srate; % Rey added this line (i.e., sampling period).
% compute wavelet
for index = 1:length(freqs)
    fk=freqs(index);
    if strcmpi(type, 'morlet') % Morlet.
        fk=fk/srate; % Normalize frequency for textbook equations as in TB97
        sigf=fk/cycles(index); % Computing time and frequency standard deviations, resolutions, and normalization constant.
        sigt=1./(2*pi*sigf);
        A=1./sqrt(sigt*sqrt(pi));
        timeresol(index)=2*sigt/srate; % sec
        freqresol(index)=2*sigf*srate; % Hz
        if isempty(winsize) % bases will be a cell array.
%             tneg=[-sp:-sp:-sigt*timesupport/2];
%             tpos=[0:sp:sigt*timesupport/2];
%             t=[fliplr(tneg) tpos];
            t = (0:floor(sigt*timesupport/2)*2)-floor(sigt*timesupport/2); % Always odd; backward compatible
            psi=A.*(exp(-(t.^2)./(2*(sigt^2))).*exp(2*i*pi*fk*t));
            wavelet{index}=psi;  % These are the wavelets with variable number of samples based on temporal standard deviations (sigt).
        else % bases will be a matrix.
%             tneg=[-sp:-sp:-sp*winsize/2];
%             tpos=[0:sp:sp*winsize/2];
%             t=[fliplr(tneg) tpos];
            t = (0:floor(winsize/2)*2)-floor(winsize/2); % Always odd; backward compatible
            psi=A.*(exp(-(t.^2)./(2*(sigt^2))).*exp(2*i*pi*fk*t));
            wavelet(index,:)=psi; % These are the wavelets with the same length.
            % This is useful for doing time-frequency analysis as a matrix vector or matrix matrix multiplication.
        end
    elseif strcmpi(type, 'sinus') % Hanning
        tneg=[-sp:-sp:-sp*winsize/2];
        tpos=[0:sp:sp*winsize/2];
        t=[fliplr(tneg) tpos];
        win = exp(2*i*pi*fk*t);
        wavelet(index,:) = win .* hanning(winsize)';
        %wavelet{index} = win .* hanning(winsize)';
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    end
end

% symmetric hanning function
function w = hanning(n)
if ~rem(n,2)
    w = .5*(1 - cos(2*pi*(1:n/2)'/(n+1)));
    w = [w; w(end:-1:1)];
else
    w = .5*(1 - cos(2*pi*(1:(n+1)/2)'/(n+1)));
    w = [w; w(end-1:-1:1)];
end