function K=kronSym(A,B)
%%KRONSYM Take the symmetric Kronecker product of the matrices A and B. The
%         standard Kronecker product of two real, square matrices A and B,
%         one can write the following relation with respect to any real
%         square matrix S:
%         kron(A,B)*vec(S)==vec(B*S*A')
%         However, the symmetric Kronecker product is such that for any two
%         real square matrices A and B and a square SYMMETRIC matrix S, one
%         can write
%         kronSym(A,B)*vech(S,sqrt(2))==vech((1/2)*(B*S*A'+A*S*B'),sqrt(2))
%         Whereas is A and B are nXn, the standard kronecker product is
%         n^2Xn^2, the symmetric Kronecker product is
%         (n*(n+1)/2)X(n*(n+1)/2).
%
%INPUTS: A, B Two real nXn matrices. They need not be square.
%
%OUTPUTS: K The (n*(n+1)/2)X(n*(n+1)/2) symmetric Kronecker product of A
%           and B.
%
%Symmetric Kronecker products are discussed in the appendix of [1], where
%they play a role in the implementation of a semidefinite programming
%algorithm.
%
%The symmetric Kronecker product is implemented by noting that in the
%identity
%kronSym(A,B)*vech(S,sqrt(2))==vech((1/2)*(B*S*A'+A*S*B'),sqrt(2)), if S is
%a symmetric matrix where on or below the main diagonal there is only a
%single nonzero element, then one effectively selects a column of
%kronSym(A,B). To get an unscaled column of kronSym(A,B), the nonzero
%element should be 1 if it is on the main diagonal and it should be
%1/sqrt(2) for elements below (and above) the main diagonal due to the
%sqrt(2) scaling in the vech command. Basically, a simple implementation
%would be
% n=size(A,1);
% prodDim=n*(n+1)/2;
%
% K=zeros(prodDim,prodDim);
% SelMat=zeros(n,n);
% for curEl=1:prodDim
%     [row,col]=vechInd2Sub(n,curEl);
%     if(row==col)
%         SelMat(row,col)=1;
%     else%row>col
%         SelMat(row,col)=1/sqrt(2);
%         SelMat(col,row)=1/sqrt(2);
%     end
%     K(:,curEl)=vech((1/2)*(B*SelMat*A'+A*SelMat*B'),sqrt(2));
%
%     SelMat(row,col)=0;
%     SelMat(col,row)=0;
% end
%However, that is rather slow. Thus, this implementation tries to directly
%write the elements of the output matrix K.
%
%REFERENCES:
%[1] F. Alizadeh, J.-P. A. Haeberly, and M. L. Overton, "Primal-dual
%    interior-point methods for semidefinite programming: Convergence
%    rates, stability and numerical results," SIAM Journal on Optimization,
%    vol. 8, no. 3, pp. 746-768, 1998.
%
%February 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The columns of K are indexed using (i1,i2) and the rows are indexed using
%(j1,j2).
n=size(A,1);
prodDim=n*(n+1)/2;

K=zeros(prodDim,prodDim);
sqrt2=sqrt(2);
dblSqrt2=2*sqrt2;
for i1=1:n
    col=i1+(i1-1)*(2*n-i1)/2;
    for j1=1:n
        row=j1+(j1-1)*(2*n-j1)/2;

        K(row,col)=(A(j1,i1)*B(j1,i1)+B(j1,i1)*A(j1,i1))/2;
        for j2=(j1+1):n
            row=row+1;

            K(row,col)=(A(j1,i1)*B(j2,i1)+B(j1,i1)*A(j2,i1))/sqrt2;
        end
    end

    for i2=(i1+1):n
      col=col+1;
      for j1=1:n
         row=j1+(j1-1)*(2*n-j1)/2;

         K(row,col)=(A(j1,i1)*B(j1,i2)+A(j1,i2)*B(j1,i1)+B(j1,i1)*A(j1,i2)+B(j1,i2)*A(j1,i1))/dblSqrt2;
         for j2=(j1+1):n
            row=row+1;

            K(row,col)=(A(j1,i1)*B(j2,i2)+A(j1,i2)*B(j2,i1)+B(j1,i1)*A(j2,i2)+B(j1,i2)*A(j2,i1))/2;
         end
      end
    end
end
end

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


function m = nanmean(x,dim)
%NANMEAN Mean value, ignoring NaNs.
%   M = NANMEAN(X) returns the sample mean of X, treating NaNs as missing
%   values.  For vector input, M is the mean value of the non-NaN elements
%   in X.  For matrix input, M is a row vector containing the mean value of
%   non-NaN elements in each column.  For N-D arrays, NANMEAN operates
%   along the first non-singleton dimension.
%
%   NANMEAN(X,DIM) takes the mean along dimension DIM of X.
%
%   See also MEAN, NANMEDIAN, NANSTD, NANVAR, NANMIN, NANMAX, NANSUM.

%   Copyright 1993-2004 The MathWorks, Inc.
%   $Revision: 1.1.8.1 $  $Date: 2010/03/16 00:15:50 $

% Find NaNs and set them to zero
nans = isnan(x);
x(nans) = 0;

if nargin == 1 % let sum deal with figuring out which dimension to use
    % Count up non-NaNs.
    n = sum(~nans);
    n(n==0) = NaN; % prevent divideByZero warnings
    % Sum up non-NaNs, and divide by the number of non-NaNs.
    m = sum(x) ./ n;
else
    % Count up non-NaNs.
    n = sum(~nans,dim);
    n(n==0) = NaN; % prevent divideByZero warnings
    % Sum up non-NaNs, and divide by the number of non-NaNs.
    m = sum(x,dim) ./ n;
end


function [xpeak, ypeak, max_f] = findpeak(f,subpixel)
%FINDPEAK Find extremum of matrix.
%   [XPEAK,YPEAK,MAX_F] = FINDPEAK(F,SUBPIXEL) finds the extremum of F,
%   MAX_F, and its location (XPEAK, YPEAK). F is a matrix. MAX_F is the maximum
%   absolute value of F, or an estimate of the extremum if a subpixel
%   extremum is requested.
%
%   SUBPIXEL is a boolean that controls if FINDPEAK attempts to estimate the
%   extremum location to subpixel precision. If SUBPIXEL is false, FINDPEAK
%   returns the coordinates of the maximum absolute value of F and MAX_F is
%   max(abs(F(:))). If SUBPIXEL is true, FINDPEAK fits a 2nd order
%   polynomial to the 9 points surrounding the maximum absolute value of
%   F. In this case, MAX_F is the absolute value of the polynomial evaluated
%   at its extremum.
%
%   Note: Even if SUBPIXEL is true, there are some cases that result
%   in FINDPEAK returning the coordinates of the maximum absolute value
%   of F:
%   * When the maximum absolute value of F is on the edge of matrix F.
%   * When the coordinates of the estimated polynomial extremum would fall
%     outside the coordinates of the points used to constrain the estimate.

%   Copyright 1993-2004 The MathWorks, Inc.
%

% get absolute peak pixel
[max_f, imax] = max(abs(f(:)));
[ypeak, xpeak] = ind2sub(size(f),imax(1));

if ~subpixel || ...
    xpeak==1 || xpeak==size(f,2) || ypeak==1 || ypeak==size(f,1) % on edge
    return % return absolute peak

else
    % fit a 2nd order polynomial to 9 points
    % using 9 pixels centered on irow,jcol
    u = f(ypeak-1:ypeak+1, xpeak-1:xpeak+1);
    u = u(:);
    x = [-1 -1 -1  0  0  0  1  1  1]';
    y = [-1  0  1 -1  0  1 -1  0  1]';

    % u(x,y) = A(1) + A(2)*x + A(3)*y + A(4)*x*y + A(5)*x^2 + A(6)*y^2
    X = [ones(9,1),  x,  y,  x.*y,  x.^2,  y.^2];

    % u = X*A
    A = X\u;

    % get absolute maximum, where du/dx = du/dy = 0
    x_offset = (-A(3)*A(4)+2*A(6)*A(2)) / (A(4)^2-4*A(5)*A(6));
    y_offset = -1 / ( A(4)^2-4*A(5)*A(6))*(A(4)*A(2)-2*A(5)*A(3));

    if abs(x_offset)>1 || abs(y_offset)>1
        % adjusted peak falls outside set of 9 points fit,
        return % return absolute peak
    end

    % return only one-tenth of a pixel precision
    x_offset = round(10*x_offset)/10;
    y_offset = round(10*y_offset)/10;

    xpeak = xpeak + x_offset;
    ypeak = ypeak + y_offset;

    % Calculate extremum of fitted function
    max_f = [1 x_offset y_offset x_offset*y_offset x_offset^2 y_offset^2] * A;
    max_f = abs(max_f);

end


function area = polygonArea3d(poly, varargin)
%POLYGONAREA3D Area of a 3D polygon
%
%   AREA = polygonArea3d(POLY)
%   POLY is given as a N-by-3 array of vertex coordinates. The resulting
%   area is positive.
%   Works also for polygons given as a cell array of polygons.
%
%   Example
%     % area of a simple 3D square
%     poly = [10 30 20;20 30 20;20 40 20;10 40 20];
%     polygonArea3d(poly)
%     ans =
%        100
%
%     % Area of a 3D mesh
%     [v f] = createCubeOctahedron;
%     polygons = meshFacePolygons(v, f);
%     areas = polygonArea3d(polygons);
%     sum(areas)
%     ans =
%         18.9282
%
%   See also
%     polygons3d, triangleArea3d, polygonArea, polygonCentroid3d

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2012-02-24,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2012 INRA - Cepia Software Platform.

%   HISTORY
%   2013-08-20 add support for multiple polygons

% Check multiple polygons
if iscell(poly) || sum(sum(isnan(poly))) > 0
    % split the polygons into a cell array
    polygons = splitPolygons3d(poly);
    nPolys = length(polygons);

    % compute area of each polygon
    area = zeros(nPolys, 1);
    for  i = 1:nPolys
        area(i) = polygonArea3d(polygons{i});
    end

    return;
end

% put the first vertex at origin (reducing computation errors for polygons
% far from origin)
v0 = poly(1, :);
poly = bsxfun(@minus, poly, v0);

% indices of next vertices
N = size(poly, 1);
iNext = [2:N 1];

% compute cross-product of each elementary triangle formed by origin and
% two consecutive vertices
cp = cross(poly, poly(iNext,:), 2);

% choose one of the triangles as reference for the normal direction
vn = vectorNorm(cp);
[tmp, ind] = max(vn); %#ok<ASGLU>
cpRef = cp(ind,:);

% compute the sign of the area of each triangle
% (need to compute the sign explicitely, as the norm of the cross product
% does not keep orientation within supporting plane)
sign_i = sign(dot(cp, repmat(cpRef, N, 1), 2));

% compute area of each triangle, using sign correction
area_i = vectorNorm3d(cp) .* sign_i;

% sum up individual triangles area
area = sum(area_i) / 2;


function cpu_timing ( n )

%*****************************************************************************80
%
%% CPU_TIMING measures execution time with CPUTIME.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CPU_TIMING:\n' );
  fprintf ( 1, '  MATLAB has a built in command called CPUTIME,\n' );
  fprintf ( 1, '  which measures the CPU time elapsed so far.\n' );
  fprintf ( 1, '  The difference of two readings measures the work done lately.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TASK #1:\n' )
  fprintf ( 1, '  Multiply two random dense matices of order N\n' );
  fprintf ( 1, '  using MATLAB''s A*B operation.\n' );

  A = rand ( n, n );
  B = rand ( n, n );

  t1 = cputime ( );
  C = A * B;
  t2 = cputime ( );

  f = n * n * n;
  w = f / 1000000;
  s = t2 - t1;
  r = w / s;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  MegaFLOPs = %d\n', w );
  fprintf ( 1, '  Elapsed CPU time  = %f\n', s );
  fprintf ( 1, '  Rate  = %f\n', r );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TASK #2:\n' )
  fprintf ( 1, '  Multiply two random dense matices of order N\n' );
  fprintf ( 1, '  using explicit loops.\n' );

  A = rand ( n, n );
  B = rand ( n, n );

  t1 = cputime ( );
  for i = 1 : n
    for k = 1 : n
      C(i,k) = 0;
      for j = 1 : n
        C(i,k) = C(i,k) + A(i,j) * B(j,k);
      end
    end
  end
  t2 = cputime ( );

  f = n * n * n;
  w = f / 1000000;
  s = t2 - t1;
  r = w / s;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  MegaFLOPs = %d\n', w );
  fprintf ( 1, '  Elapsed CPU time  = %f\n', s );
  fprintf ( 1, '  Rate  = %f\n', r );

  return
end


function cvx_optval = det_rootn( X )

%DET_ROOTN nth-root of the determinant of an SPD matrix.
%   For a square matrix X, DET_ROOTN(X) returns
%       POW(DET(X),1/(size(X,1))
%   if X is symmetric (real) or Hermitian (complex) and positive semidefinite,
%   and -Inf otherwise.
%
%   This function can be used in many convex optimization problems that call for
%   LOG(DET(X)) instead. For example, if the objective function contains nothing
%   but LOG(DET(X)), it can be replaced with DET_ROOTN(X), and the same optimal
%   point will be produced.
%
%   Disciplined convex programming information:
%       DET_ROOTN is concave and nonmonotonic; therefore, when used in
%       CVX specifications, its argument must be affine.

error( nargchk( 1, 1, nargin ) );
if ndims( X ) > 2 || size( X, 1 ) ~= size( X, 2 ),
    error( 'Second argument must be a square matrix.' );
end
err = X - X';
X   = 0.5 * ( X + X' );
if norm( err, 'fro' ) > 8 * eps * norm( X, 'fro' ),
    cvx_optval = -Inf;
else
    [R,p] = chol(X);
    if p > 0,
        cvx_optval = geo_mean(eig(full(X)));
    else
        cvx_optval = geo_mean(diag(R)).^2;
    end
end

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


function yc = pwl_approx_1d ( nd, xd, yd, nc, xc )

%*****************************************************************************80
%
%% PWL_APPROX_1D determines the control values for a PWL approximant.
%
%  Discussion:
%
%    The piecewise linear approximant is defined by NC control pairs
%    (XC(I),YC(I)) and approximates ND data pairs (XD(I),YD(I)).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ND, the number of data points.
%    ND must be at least 1.
%
%    Input, real XD(ND,1), the data points.
%
%    Input, real YD(ND,1), the data values.
%
%    Input, integer NC, the number of control points.
%    NC must be at least 1.
%
%    Input, real XC(NC,1), the control points.
%
%    Output, real YC(NC,1), the control values.
%

%
%  Define the NDxNC linear system that determines the control values.
%
  a = pwl_approx_1d_matrix ( nd, xd, yd, nc, xc );
%
%  Solve the system.
%
  yc = a \ yd;

  return
end


% peq.m - Parametric EQ with matching gain at Nyquist frequency
% Author: Sophocles J. Orfanidis,
% Usage:  [b, a] = peq(G0, G, w0, Dw, NdB)
%
% G0 = reference gain at DC in dB
% G  = boost/cut gain in dB
% GB = bandwidth gain in dB
%
% w0 = center frequency in Hz
% Dw = bandwidth in Hz
% NdB = amount of db less than the peak/notch gain as the bandwidth frequencies
% b  = [b0, b1, b2] = numerator coefficients
% a  = [1,  a1, a2] = denominator coefficients

% Modified by Arvind using the equations from IIR Filter Design Chapter in
% the book
function [b, a, beta] = peq(G0, G, w0, Dw, NdB)

%Convert from Decibels to real values
GB = G * NdB;
G  = (10^(G/20));
GB = (10^(GB/20));
G0 = (10^(G0/20));

%Convert absolute frequencies to rads/sec
%w0 = w0*2*pi/fs;
%Dw = Dw*2*pi/fs;

beta = sqrt((GB^2-G0^2)/(G^2-GB^2)) * tan(Dw/2);
b = [(G0+G*beta)/(1+beta) -(2*G0*cos(w0))/(1+beta)  (G0-G*beta)/(1+beta)];
a = [1 -(2*cos(w0))/(1+beta) (1-beta)/(1+beta)];

%***********************
% double beta;
%
% beta = sqrt((GB[0]^2-G0[0]^2)/(G^2[0]-GB[0]^2)) * tan(Dw[0]/2)
% Num[0] = (G0[0]+G[0]*beta)/(1+beta);
% Num[1] =  -(2*G0[0]*cos(w0[0]))/(1+beta) ;
% Num[2] = (G0[0]-G[0]*beta)/(1+beta);
%
% Den[0] = 1;
% Den[1] = -(2*cos(w0[0]))/(1+beta);
% Den[2] = (1-beta)/(1+beta);
%*************************


%% Transform Compression JPEG

% JPEG Baseline Encoder

% Jorge Calderon <calderonjorge9@gmail.com>
% University Of Antioquia
% Created: November 2012
% Modified: January 2013
% Copyright 2012
% All rights reserved

%% Clean Workplace

clear all;
close all;
clc;

%% Initialize Variables

check = 0;
stream=[];

DCTQ=[...
    16 11 10 16 24 40 51 61;...
    12 12 14 19 26 58 60 55;...
    14 13 16 24 40 57 69 56;...
    14 17 22 29 51 87 80 62;...
    18 22 37 56 68 109 103 77;...
    24 36 55 64 81 194 113 92;...
    49 64 78 87 103 121 120 101;...
    72 92 95 98 112 100 103 99];

z=[...
    9 2 3 10 17 25 18 11 4 5 12 19 26 ...
    33 41 34 27 20 13 6 7 14 21 28 35 ...
    42 49 57 50 43 36 29 22 15 8 16 23 ...
    30 37 44 51 58 59 52 45 38 31 24 32 ...
    39 46 53 60 61 54 47 40 48 55 62 63 56 64];

%% Read Image

while check<1
    [img_in pathname]=uigetfile({'*.bmp;*.tif','Image(*.bmp,*.tif)'},'Select Uncompressed Image','Multiselect','off');
    if img_in>0
        cd(pathname)
        image = double(imread(img_in));
        [row, col, dim] = size(image);
        if col<row
            disp('Error: The image must be horizontal');
            disp('Press any key to continue...');
            pause;
        else
            if (row<720 && col<1280)
                check=1;
            else
                disp('Error: The image size is too large');
                disp('Press any key to continue...');
                pause;
            end
        end
    else
        if img_in==0
            return;
        end
    end
end

%% Check Size

if mod(row,2)~=0
    row = row-1;
end
if mod(col,2)~=0
    col = col-1;
end
image = imresize(image, [row col]);

%% Convert Image To YCBCR

img = rgb2ycbcr(image);

%% Adjust Aspect Ratio

if mod(row,72)~=0
    row_aspect = row+(72-mod(row,72));
else
    row_aspect = row;
end
col_aspect = (row_aspect/9)*16;
image_aspect = zeros(row_aspect, col_aspect, dim);
zig_zag_ac = zeros(row_aspect*col_aspect/64, 63);
zig_zag_dc = zeros(row_aspect*col_aspect/64, 1);

for i=1:col
    for j=1:row
        for k=1:dim
            image_aspect(j+abs(row_aspect-row)/2,i+abs(col_aspect-col)/2,k)=img(j,i,k);
        end
    end
end

% figure(1)
% imshow(uint8(image_aspect));

%% Digitization Scheme 420

comp_blue = image_aspect(:,:,2);
comp_red = image_aspect(:,:,3);
luminance = double(image_aspect(:,:,1))-128;
imgcb_420 = double(comp_blue(1:2:size(comp_blue,1),1:2:size(comp_blue,2)))-128;
imgcr_420 = double(comp_red(1:2:size(comp_red,1),1:2:size(comp_red,2)))-128;

%% Segmenting Image Blocks Of 8x8

k=0;
for i=1:8:row_aspect
    for j=1:8:col_aspect
        f=luminance(i:i+7,j:j+7);
        T=dct2(f);
        TQ=round(T./DCTQ);
        k=k+1;
        zig_zag_dc(k,1) = TQ(1,1);
        zig_zag_ac(k,1:63) = TQ(z);
    end
end

%% Huffman Compression

dpcm(1,1)=zig_zag_dc(1,1);
stream=cat(2,stream,huffman_dc(dpcm(1,1)),huffman_ac(zig_zag_ac(1,1:63)));
for m=2:k
    dpcm(m,1)=zig_zag_dc(m,1)-zig_zag_dc(m-1,1);
    stream=cat(2,stream,huffman_dc(dpcm(m,1)),huffman_ac(zig_zag_ac(m,1:63)));
end

%% Byte Stuffing

p=0;
G=size(stream,2);
for i=1:8:size(stream,2)
    bit_val(1,1:8)=stream(1,i:i+7);
    if strcmp(bit_val(1,1:8),'11111111')==1
        tempbitstream = stream(1,i+8:G+p);
        stream(1,i+8:i+15)='00000000';
        p=p+8;
        temp2_bitstream=stream(1,1:i+15);
        stream(1,1:G+p)=cat(2,temp2_bitstream,tempbitstream);
    end
end

%% Convert String To Decimal

numbytes=floor(length(stream)/8);
diff_stream=length(stream)-numbytes*8;
if diff_stream==0
    matrix_code_decimal= zeros(numbytes+2,8);
else
    matrix_code_decimal= zeros(numbytes+3,8);
end
s=0;
for count2=1:8:numbytes*8
    s=s+1;
    matrix_code_decimal(s,1)=bin2dec(stream(1,count2:count2+7));
end

if diff_stream~=0
    s=s+1;
    matrix_code_decimal(s,1)=bin2dec(stream(1,numbytes*8+1:length(stream)));
end

%% Final Marker

matrix_code_decimal(s+1,1)=255;
matrix_code_decimal(s+2,1)=217;

%% Header JFIF

signal=[255	216	255	224	000	016	074	070	073	070	000	001	002	000	000	096	000	096	000	000	...
        255	219	000	067	000	016	011	012	014	012	010	016	014	013	014	018	017	016	019	024	...
        040	026	024	022	022	024	049	036	037	029	040	058	051	061	060	057	051	056	055	064	...
        072	092	078	064	068	087	069	055	056	080	109	081	087	095	098	103	194	103	062	077	...
        113	121	112	100	120	092	101	103	099	255	219	000	067	001	017	018	018	024	021	024	...
        047	026	026	047	099	066	056	066	099	099	099	099	099	099	099	099	099	099	099	099	...
        099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	...
        099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	255	192	...
        000	011	008	000	000	000	000	001	001	034	000	255	196	000	031	000	000	001	005	001	...
        001	001	001	001	001	000	000	000	000	000	000	000	000	001	002	003	004	005	006	007	...
        008	009	010	011	255	196	000	181	016	000	002	001	003	003	002	004	003	005 	005	004	...
        004	000	000	001	125	001	002	003	000	004	017	005	018	033	049	065	006	019	081	097	...
        007	034	113	020	050	129	145	161	008	035	066	177	193	021	082	209	240	036	051	098	...
        114	130	009	010	022	023	024	025	026	037	038	039	040	041	042	052	053	054	055	056	...
        057	058	067	068	069	070	071	072	073	074	083	084	085	086	087	088	089	090	099	100	...
        101	102	103	104	105	106	115	116	117	118	119	120	121	122	131	132	133	134	135	136	...
        137	138	146	147	148	149	150	151	152	153	154	162	163	164	165	166	167	168	169	170	...
        178	179	180	181	182	183	184	185	186	194	195	196	197	198	199	200	201	202	210	211	...
        212	213	214	215	216	217	218	225	226	227	228	229	230	231	232	233	234	241	242	243	...
        244	245	246	247	248	249	250	255	218	000	008	001	001	000	000	063	000];

% Start of Image (SOI) marker:FFD8=255,216
% JFIF marker:FFE0=255,224
%        Length=000,016
%        Identifier:4A46494600=074,070,073,070,000
%        Version=001,002
%        Units=000
%        Xdensity=000,096
%        Ydensity=000,096
%        Xthumbnail=000
%        Ythumbnail=000
%        (RGB)n, n=Xthumbnail*Ythumbnail required 3*n bytes=null

% Define Quantization table marker (luma):FFDB=255,219
%         Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,067
%         Precision=000 (baseline)
%         Quantization values=016,011,012,014,012,010,016,...,101,103,099 (zigzag)

% Define Quantization table marker (Chroma):FFDB=255,219
%         Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,067
%         Precision=000 (baseline)
%         Quantization values=017,018,018,024,021,024,047,...,099,099,099 (zigzag)

% Start of frame marker:FFC0=255,192
%         Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,011
%         Sample precision=008
%         X=000,000 (This will be defined later)
%         Y=000,000 (This will be defined later)
%         Number of components in the image=001
%             * 3 for color baseline
%             * 1 for grayscale baseline
%         Component ID=001
%         H and V sampling factors=034
%         Quantization table number=000

% Define Huffman table marker (DC):FFC4=255,196
%        Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,031
%        Index=000 (Huffman DC)
%        Bits=The next 16 bytes from an array of unsigned 1-byte integers whose elements give the number of Huffman codes for each possible code length (1-16).
%        Huffman values=000,001,002,...,010,011

% Define Huffman table marker (AC):FFC4=255,196
%        Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,181
%        Index=016 (Huffman AC)
%        Bits=The next 16 bytes from an array of unsigned 1-byte integers whose elements give the number of Huffman codes for each possible code length (1-16).
%        Huffman values=001,002,003,...,249,250

% Start of Scan marker:FFDA=255,218
%        Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,008
%        Number of components=001
%        Component ID=001
%        DC and AC table numbers=000
%        Ss=000
%        Se=063
%        Ah and Al=000

%% Define Size Image

Y = dec2hex(row_aspect,4);
X = dec2hex(col_aspect,4);
signal(1,164) = hex2dec(Y(1,1:2));
signal(1,165) = hex2dec(Y(1,3:4));
signal(1,166) = hex2dec(X(1,1:2));
signal(1,167) = hex2dec(X(1,3:4));

%% Concatenate Coding + Header

JP_STREAM(1,1:size(signal,2))=signal(1,1:size(signal,2));
for j=1:1:size(matrix_code_decimal,1)
    JP_STREAM(1,size(signal,2)+j)=matrix_code_decimal(j,1);
end

%% JPG Data Store

JP_STREAM=JP_STREAM';
img_name = regexp(img_in,'\.','split');
img_name = img_name{1};
fid = fopen([img_name '.jpg'], 'wb');
if fid < 0
    error('Failed to open data file for write');
end
fwrite(fid,JP_STREAM,'uint8');
fclose(fid);


function v = planeIntersect(N1,N2,alpha1,alpha2)
% compute the intersection of two planes

d = dot(N1,N2);
c1 = (alpha1 - alpha2 .* d)./(1-d.^2);
c2 = (alpha2 - alpha1 .* d)./(1-d.^2);

% the line intersecting both planes is
% a + lambda b
a = c1 .* N1 + c2 .* N2;
b = cross(N1,N2);

% determine lambda such that norm(a + lambda b) = 1
p = dot(a,b) ./ norm(b).^2;
q = (norm(a).^2 - 1)./norm(b).^2;

lambda1 = - p + sqrt(p.^2 - q);
lambda2 = - p - sqrt(p.^2 - q);

% compute the points on the sphere
v = [a(:) + lambda1(:) .* b(:), a(:) + lambda2(:) .* b(:)];

end


function [ t, wts ] = cgqfs ( nt, kind, alpha, beta, lo )

%*****************************************************************************80
%
%% CGQFS computes knots and weights of a Gauss quadrature formula.
%
%  Discussion:
%
%    This routine computes the knots and weights of a Gauss quadrature
%    formula with:
%
%    * a classical weight function with default values for a,b
%    * only simple knots
%    * optionally print knots and weights and a check of the moments
%
%    Use routine EIQFS to evaluate a quadrature formula computed by
%    this routine.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer NT, the number of knots.
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev Type 1,     (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,+oo)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-oo,+oo)   |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,+oo)     (x-a)^alpha*(x+b)^beta
%    9, Chebyshev Type 2,     (a,b)       ((b-x)*(x-a))^(+0.5)
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Input, integer LO, selects the action.
%    > 0, compute and print knots and weights.  Print moments check.
%    = 0, compute knots and weights.
%    < 0, compute and print knots and weights.
%
%    Output, real T(NT), the knots.
%
%    Output, real WTS(NT), the weights.
%

%
%  Compute the Gauss quadrature formula.
%
  [ t, wts ] = cdgqf ( nt, kind, alpha, beta );
%
%  Exit if no print required.
%
  if ( lo == 0 )
    return
  end

  key = 1;
  mop = 2 * nt;
  m = mop + 1;
  mmex = max ( 2 * nt + 3, 1 );
%
%  All knots have multiplicity = 1.
%
  mlt = zeros(nt,1);
  mlt(1:nt) = 1;
%
%  NDX(I) = I.
%
  ndx = ( 1 : nt );

  w = zeros(m,1);

  chkqfs ( t, wts, mlt, nt, nt, ndx, key, mop, mmex, ...
    kind, alpha, beta, lo, w );

  return
end


function learningConvergence

k = 10;
iter = 1:200;

lblRate  = LowerBoundLipshitzFirstOrder(iter);
lbscRate = lowerBoundStronglyConvexFirstOrder(iter,k);
sRate = subgradientConvergence(iter);
nRate = stronglyConvexNesterovConvergence(iter,k);
qRate = quadraticConvergence(iter,1/k);
lgRate = LipschitzGradientConvergence(iter);
scRate = stronglyConvexGradientConvergence(iter,k);

colors =    {
            [0.9290, 0.6940, 0.1250];
            [0, 0.4470, 0.7410];
          	[0.8500, 0.3250, 0.0980];
            [0, 0.4470, 0.7410];
            [0.8500, 0.3250, 0.0980];
          	[0.4940, 0.1840, 0.5560];
          	[0.4660, 0.6740, 0.1880];
          	[0.3010, 0.7450, 0.9330]};
style = {'-';'--';'--';'-';'-';'-';'-';};

rates = [sRate;lblRate;lbscRate;lgRate;scRate;nRate;qRate];

figure(1)
subplot(1,2,1);
hold on
for i = 1:size(rates,1)
    plot(iter,rates(i,:),'Color',colors{i},'LineStyle',style{i});
end
legend('Subgradient','LowerBound Lipshitz','LowerBound Strongly Convex', 'Lipschitz Diff Gradient','StronglyConvex Diff Gradient','Strongly Convex Diff Nesterov','Quadratic Diff')

figure(1)
subplot(1,2,2);
hold on
for i = 1:size(rates,1)
h = plot(iter,rates(i,:),'Color',colors{i},'LineStyle',style{i});
set(gca,'yscale','log');
end

legend('Subgradient','LowerBound Lipshitz','LowerBound Strongly Convex', 'Lipschitz Diff Gradient','StronglyConvex Diff Gradient','Strongly Convex Diff Nesterov','Quadratic Diff')

end

function rate = subgradientConvergence(x)

rate = 1./sqrt(x);

end

function rate = LowerBoundLipshitzFirstOrder(x)

rate = 1./(x + 1).^2;

end

function rate = LipschitzGradientConvergence(x)

rate = 1./(x + 1);

end

function rate = stronglyConvexGradientConvergence(x,k)

rate = (((k) - 1)/((k)+1)).^(x);
end

function rate = quadraticConvergence(x,k)

rate = k.^(2.^x);

end

function rate = lowerBoundStronglyConvexFirstOrder(x,k)

rate = ((sqrt(k) - 1)/(sqrt(k) + 1)).^(2*x);

end

function rate = stronglyConvexNesterovConvergence(x,k)

rate = ((sqrt(k) - 1)/(sqrt(k)+1)).^(x);

end


function value = r8_lgmc ( x )

%*****************************************************************************80
%
%% R8_LGMC evaluates the log gamma correction factor for an R8 argument.
%
%  Discussion:
%
%    For 10 <= X, compute the log gamma correction factor so that
%
%      log ( gamma ( x ) ) = log ( sqrt ( 2 * pi ) )
%                          + ( x - 0.5 ) * log ( x ) - x
%                          + r8_lgmc ( x )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the correction factor.
%
  persistent algmcs
  persistent nalgm
  persistent xbig
  persistent xmax

  if ( isempty ( nalgm ) )

    algmcs = [ ...
      +0.1666389480451863247205729650822, ...
      -0.1384948176067563840732986059135E-04, ...
      +0.9810825646924729426157171547487E-08, ...
      -0.1809129475572494194263306266719E-10, ...
      +0.6221098041892605227126015543416E-13, ...
      -0.3399615005417721944303330599666E-15, ...
      +0.2683181998482698748957538846666E-17, ...
      -0.2868042435334643284144622399999E-19, ...
      +0.3962837061046434803679306666666E-21, ...
      -0.6831888753985766870111999999999E-23, ...
      +0.1429227355942498147573333333333E-24, ...
      -0.3547598158101070547199999999999E-26, ...
      +0.1025680058010470912000000000000E-27, ...
      -0.3401102254316748799999999999999E-29, ...
      +0.1276642195630062933333333333333E-30 ]';

    nalgm = r8_inits ( algmcs, 15, r8_mach ( 3 ) );
    xbig = 1.0 / sqrt ( r8_mach ( 3 ) );
    xmax = exp ( min ( log ( r8_mach ( 2 ) / 12.0 ), ...
      - log ( 12.0 * r8_mach ( 1 ) ) ) );
  end

  if ( x < 10.0 )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_LGMC - Fatal error!\n' );
    fprintf ( 1, '  X must be at least 10.\n' );
    error ( 'R8_LGMC - Fatal error!' )

  elseif ( x < xbig )

    value = r8_csevl ( 2.0 * ( 10.0 / x ) ...
      * ( 10.0 / x ) - 1.0, algmcs, nalgm ) / x;

  elseif ( x < xmax )

    value = 1.0 / ( 12.0 * x );

  else

    value = 0.0;

  end

  return
end


function [ a, q, lambda, seed ] = r8symm_test ( n, lambda_mean, lambda_dev, ...
  seed )

%*****************************************************************************80
%
%% R8SYMM_TEST determines a symmetric matrix with a certain eigenstructure.
%
%  Discussion:
%
%    An R8SYMM is a real symmetric matrix stored using full storage, and
%    using R8 arithmetic.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real LAMBDA_MEAN, the mean value of the normal
%    distribution from which the eigenvalues will be chosen.
%
%    Input, real LAMBDA_DEV, the standard deviation of the normal
%    distribution from which the eigenvalues will be chosen.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real A(N,N), the test matrix.
%
%    Output, real Q(N,N), the eigenvector matrix.
%
%    Output, real LAMBDA(N), the eigenvalue vector.
%

%
%  Choose the eigenvalues LAMBDA.
%
  [ lambda, seed ] = r8vec_normal ( n, lambda_mean, lambda_dev, seed );
%
%  Get a random orthogonal matrix Q.
%
  [ q, seed ] = r8mat_orth_uniform ( n, seed );
%
%  Set A = Q * Lambda*I * Q'.
%
  a = q * diag ( lambda ) * q';

  return
end


function a=v_polygonarea(p)
%V_POLYGONAREA Calculate the area of a polygon
%
% Inputs:
%    P(n,2) is the polygon vertices
%
% Outputs:
%    A is teh area of teh polygon
%
% The area is positive if the vertices go anti-clockwise around the polygon.
%

%      Copyright (C) Mike Brookes 2009
%      Version: $Id: v_polygonarea.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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
p(end+1,:)=p(1,:);      % append an extra point
a=0.5*sum((p(1:end-1,1)-p(2:end,1)).*(p(1:end-1,2)+p(2:end,2)),1);
if ~nargout
    plot(p(:,1),p(:,2),'b-');
    mnx=[1.05 -0.05;-0.05 1.05]*[min(p);max(p)];
    set(gca,'xlim',mnx(:,1)','ylim',mnx(:,2)');
    title(sprintf('Area = %.2g',a));
end


%  Figure 3.30e      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%

clf;
num=1;
a=.5;

zeta =1;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
t=0:.1:10;
y1=step(num,den,t);

zeta =.7;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
y2=step(num,den,t);

zeta =.5;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
y3=step(num,den,t);

axis([1 10 .1 .9])
plot(t,y1,'-',t,y2,'--',t,y3,'-.'),grid
title('Fig. 3.30e Step response with extra pole, \alpha= .5')
xlabel('\omega_n t')
ylabel('y(t)')


function [G]=gsp_low_stretch_tree(k)
%GSP_LOW_STRETCH_TREE  Initialize a low stretch tree
%   Usage:  G = gsp_low_stretch_tree(k);
%           G = gsp_low_stretch_tree();
%
%   Input parameters:
%         k     : 2^k points on each side of the grid of vertices. (default 6)
%   Output parameters:
%         G     : Graph structure.
%
%   'gsp_create_low_stretch_tree(k)' initializes a graph structure containing
%   the weighted adjacency matrix (G.W), the number of vertices (G.N), the
%   plotting coordinates (G.coords), the plotting coordinate limits
%   (G.limits), and the root of a low stretch tree on a grid of points.
%   There are $2^k$ points on each side of the grid, and therefore $2^{2k}$
%   total vertices. The edge weights are all equal to 1.
%
%   Example:::
%
%          G = gsp_low_stretch_tree(3);
%          paramplot.show_edges = 1;
%          gsp_plot_graph(G,paramplot);
%
%   See also: gsp_graph

% Author : David I Shuman, Nathanael Perraudin

if nargin < 1
   k = 6;
end

ii=[2 3 1 1 4 3];
jj=[1 1 2 3 3 4];

XCoords=[1, 2, 1, 2];
YCoords=[1, 1, 2, 2];

for p=2:k
    % create the indicies of the weight matrice
    ii_new=[ii,ii+4^(p-1),ii+2*4^(p-1),ii+3*4^(p-1)];
    ii_new_middle=[4^(p-1),4^(p-1),4^(p-1)+(4^p+2)/3,5/3*4^(p-1)+1/3,4^(p-1)+(4^p+2)/3,3*4^(p-1)+1];
    ii=[ii_new,ii_new_middle];

    jj_new=[jj,jj+4^(p-1),jj+2*4^(p-1),jj+3*4^(p-1)];
    jj_new_middle=[5/3*4^(p-1)+1/3,4^(p-1)+(4^p+2)/3,3*4^(p-1)+1,4^(p-1),4^(p-1),4^(p-1)+(4^p+2)/3];
    jj=[jj_new,jj_new_middle];

    % Create Coords
    YCoords=repmat(YCoords,1,2);
    YCoords_new=[YCoords,YCoords+2^(p-1)];
    YCoords=YCoords_new;

    XCoords_new=[XCoords,XCoords+2^(p-1)];
    XCoords=repmat(XCoords_new,1,2);
end

G.W=sparse(ii,jj,ones(size(ii)));
G.coords=[XCoords',YCoords'];

G.limits=[0,2^k+1,0,2^k+1];
G.N=(2^k)^2;
G.root=4^(k-1);

G.plotting.edge_width=1.25;
G.plotting.vertex_size=75;

G.type = 'low strech tree';

G = gsp_graph_default_parameters(G);

end


function y=rdct(x,n,a,b)
%RDCT     Discrete cosine transform of real data Y=(X,N,A,B)
% Data is truncated/padded to length N.
%
% This routine is equivalent to multiplying by the matrix
%
%   rdct(eye(n)) = diag([sqrt(2)*B/A repmat(2/A,1,n-1)]) * cos((0:n-1)'*(0.5:n)*pi/n)
%
% Default values of the scaling factors are A=sqrt(2N) and B=1 which
% results in an orthogonal matrix. Other common values are A=1 or N and/or B=1 or sqrt(2).
% If b~=1 then the columns are no longer orthogonal.
%
% see IRDCT for the inverse transform

% BUG: in line 51 we should do chopping after transform and not before

fl=size(x,1)==1;
if fl x=x(:); end
[m,k]=size(x);
if nargin<2 n=m;
end
if nargin<4 b=1;
    if nargin<3 a=sqrt(2*n);
    end
    end
if n>m x=[x; zeros(n-m,k)];
elseif n<m x(n+1:m,:)=[];
end

x=[x(1:2:n,:); x(2*fix(n/2):-2:2,:)];
z=[sqrt(2) 2*exp((-0.5i*pi/n)*(1:n-1))].';
y=real(fft(x).*z(:,ones(1,k)))/a;
y(1,:)=y(1,:)*b;
if fl y=y.'; end


function HTotal=uHessian3D(xG,lRx,M)
%%UHESSIAN3D Determine the Hessian matrix (second derivative matrix) of a
%           a single direction cosine u component in 3D with respect to 3D
%           position. Relativity and atmospheric effects are not taken into
%           account. This function is essentially the same as uvHessian
%           without the v component.
%
%INPUTS: xG The 3XN target position vectors in the global coordinate system
%           with [x;y;z] components for which gradients are desired.
%       lRx The 3X1  position vector of the receiver. If omitted, the
%           receiver is placed at the origin.
%         M A 3X3 rotation matrix from the global Coordinate system to the
%           orientation of the coordinate system at the receiver. If
%           omitted, it is assumed to be the identity matrix.
%
%OUTPUTS: HTotal A 3X3XN matrix such that HTotal(:,:,i) is the Hessian
%          matrix with respect to the u component of the ith measurement.
%          The elements in the matrices for each component/ point are
%          ordered [d^2/(dxdx), d^2/(dxdy), d^2/(dxdz);
%                   d^2/(dydx), d^2/(dydy), d^2/(dydz);
%                   d^2/(dzdx), d^2/(dzdy), d^2/(dzdz)];
%          note that each matrix is symmetric (i.e.
%           d^2/(dydx)=d^2/(dxdy) ).
%
%A derivation of the components of the Jacobian is given in [1]. The
%Hessian is just one derivative higher.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%July 2021 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(M))
	M=eye(3,3);
end

if(nargin<2||isempty(lRx))
	lRx=zeros(3,1);
end

N=size(xG,2);
HTotal=zeros(3,3,1,N);
for curPoint=1:N
    %Convert the state into the local coordinate system.
    xLocal=M*(xG(1:3)-lRx(1:3));

    x=xLocal(1);
    y=xLocal(2);
    z=xLocal(3);

    r5=norm(xLocal)^5;

    H=zeros(3,3,1);

    %u Hessian values
    %du/(dxdx)
    H(1,1,1)=-((3*x*(y^2+z^2))/r5);
    %du/(dydy)
    H(2,2,1)=-((x*(x^2-2*y^2+z^2))/r5);
    %du/(dzdz)
    H(3,3,1)=-((x*(x^2+y^2-2*z^2))/r5);
    %du/(dxdy)
    H(1,2,1)=-((y*(-2*x^2+y^2+z^2))/r5);
    %du/(dydx)
    H(2,1,1)=H(1,2,1);
    %du/(dxdz)
    H(1,3,1)=-((z*(-2*x^2+y^2+z^2))/r5);
    %du/(dzdx)
    H(3,1,1)=H(1,3,1);
    %du/(dydz)
    H(2,3,1)=(3*x*y*z)/r5;
    %du/(dzdy)
    H(3,2,1)=H(2,3,1);

    %Rotate the values back into global coordinates
    HTotal(:,:,1,curPoint)=M'*H(:,:,1)*M;
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 out = CO_trev(y,tau)
% CO_trev   Normalized nonlinear autocorrelation, trev function of a time series
%
% Calculates the trev function, a normalized nonlinear autocorrelation,
% mentioned in the documentation of the TSTOOL nonlinear time-series analysis
% package (available here: http://www.physik3.gwdg.de/tstool/).
%
% The quantity is often used as a nonlinearity statistic in surrogate data
% analysis, cf. "Surrogate time series", T. Schreiber and A. Schmitz, Physica D,
% 142(3-4) 346 (2000).
%
%---INPUTS:
%
% y, time series
%
% tau, time lag (can be 'ac' or 'mi' to set as the first zero-crossing of the
%       autocorrelation function, or the first minimum of the automutual
%       information function, respectively)
%
%---OUTPUTS:
% The raw trev expression, its magnitude, the numerator and its magnitude, and
% the denominator.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

%-------------------------------------------------------------------------------
%% Set defaults:
if nargin < 2 || isempty(tau)
    tau = 'ac';
end

%-------------------------------------------------------------------------------
% Can set the time lag, tau, to be 'ac' or 'mi':
if strcmp(tau,'ac')
    tau = CO_FirstCrossing(y,'ac',0,'discrete');
    % tau is first zero crossing of the autocorrelation function
elseif strcmp(tau,'mi')
    tau = CO_FirstMin(y,'mi');
    % tau is the first minimum of the automutual information function
end
if isnan(tau)
    error('No valid setting for time delay. (Is the time series too short?)');
end

%-------------------------------------------------------------------------------
% Compute trev quantities:

yn = y(1:end-tau);
yn1 = y(1+tau:end); % yn, tau steps ahead

% The trev expression used in TSTOOL:
out.raw = mean((yn1-yn).^3)/(mean((yn1-yn).^2))^(3/2);

% The magnitude
out.abs = abs(out.raw);

% The numerator
out.num = mean((yn1-yn).^3);
out.absnum = abs(out.num);

% The denominator
out.denom = (mean((yn1-yn).^2))^(3/2);

end


function best_weights = GetBestMixingWeight(stand_dev_of_variables)
% Best mixing of random variables with common mean
% and built-in constraint
%   best_weights = GetBestMixingWeight(stand_dev_of_variables)
%
% Input argument
%   stand_dev_of_variables: standard deviation of each variable
%
% Output argument
%   best_weights: best set of weights for mixing random variables

% Copyright 2016 Google Inc. All Rights Reserved.
% Author: hidekik@google.com (Hideki Kawahara)

narginchk(1, 1);
n_variables = length(stand_dev_of_variables);
H = ones(n_variables - 1, n_variables - 1) * ...
  stand_dev_of_variables(n_variables) ^ 2;
H = H + diag(stand_dev_of_variables(1 : n_variables - 1) .^ 2);
v = ones(n_variables - 1, 1) * stand_dev_of_variables(n_variables) ^ 2;
a = H \ v;
best_weights = [a; 1 - sum(a)];
end


function [LS,LD,I] = isolines(V,F,S,iso)
  % ISOLINES compute a list of isolines for a scalar field S defined on the
  % mesh (V,F)
  %
  % [LS,LD,I] = isolines(V,F,S,iso)
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by 3 list of triangle indices
  %   S  #V list of scalar values defined on V
  %   iso #iso list of iso values
  % Outputs:
  %   LS  #L by dim list of isoline start positions
  %   LD  #L by dim list of isoline destination positions
  %   I  #L list of indices into iso revealing corresponding iso value
  %
  % alternative: tracing isolines should result in smaller plot data (every
  % vertex only appears once
  %
  % Example:
  %   iso = linspace(min(S),max(S),10);
  %   [LS,LD] = isolines(V,F,S,iso);
  %   colormap(jet(numel(iso)-1));
  %   tsurf(F,V,'CData',S,fphong);
  %   hold on;
  %   plot([LS(:,1) LD(:,1)]',[LS(:,2) LD(:,2)]', ...
  %     'Color','k','LineWidth',10);
  %   hold off;
  %

  % make sure iso is a ROW vector
  iso = iso(:)';
  % number of isolines
  niso = numel(iso);

  % number of domain positions
  n = size(V,1);
  % number of dimensions
  dim = size(V,2);

  % number of faces
  m = size(F,1);

  % Rename for convenience
  S1 = S(F(:,1),:);
  S2 = S(F(:,2),:);
  S3 = S(F(:,3),:);

  % t12(i,j) reveals parameter t where isovalue j falls on the line from
  % corner 1 to corner 2 of face i
  t12 = bsxfun(@rdivide,bsxfun(@minus,iso,S1),S2-S1);
  t23 = bsxfun(@rdivide,bsxfun(@minus,iso,S2),S3-S2);
  t31 = bsxfun(@rdivide,bsxfun(@minus,iso,S3),S1-S3);

  % replace values outside [0,1] with NaNs
  t12( (t12<-eps)|(t12>(1+eps)) ) = NaN;
  t23( (t23<-eps)|(t23>(1+eps)) ) = NaN;
  t31( (t31<-eps)|(t31>(1+eps)) ) = NaN;

  % masks for line "parallel" to 12
  l12 = ~isnan(t23) & ~isnan(t31);
  l23 = ~isnan(t31) & ~isnan(t12);
  l31 = ~isnan(t12) & ~isnan(t23);

  % find non-zeros (lines) from t23 to t31
  [F12,I12,~] = find(l12);
  [F23,I23,~] = find(l23);
  [F31,I31,~] = find(l31);
  % indices directly into t23 and t31 corresponding to F12 and I12
  %ti12 = sub2ind(size(l12),F12,I12);
  %ti23 = sub2ind(size(l23),F23,I23);
  %ti31 = sub2ind(size(l31),F31,I31);
  % faster sub2ind
  ti12 = F12+(I12-1)*size(l12,1);
  ti23 = F23+(I23-1)*size(l23,1);
  ti31 = F31+(I31-1)*size(l31,1);

  % compute actual position values
  LS = [ ...
    ... % average of vertex positions between 2 and 3
    bsxfun(@times,1-t23(ti12), V(F(F12,2),:)) + ...
    bsxfun(@times,  t23(ti12), V(F(F12,3),:)) ...
    ; ... % average of vertex positions between 2 and 3
    bsxfun(@times,1-t31(ti23), V(F(F23,3),:)) + ...
    bsxfun(@times,  t31(ti23), V(F(F23,1),:)) ...
    ;... % average of vertex positions between 2 and 3
    bsxfun(@times,1-t12(ti31), V(F(F31,1),:)) + ...
    bsxfun(@times,  t12(ti31), V(F(F31,2),:)) ...
    ];
  LD = [...
    ... % hcat with average of vertex positions between 3 and 1
    bsxfun(@times,1-t31(ti12), V(F(F12,3),:)) + ...
    bsxfun(@times,  t31(ti12), V(F(F12,1),:)) ...
    ;... % hcat with average of vertex positions between 3 and 1
    bsxfun(@times,1-t12(ti23), V(F(F23,1),:)) + ...
    bsxfun(@times,  t12(ti23), V(F(F23,2),:)) ...
    ;... % hcat with average of vertex positions between 3 and 1
    bsxfun(@times,1-t23(ti31), V(F(F31,2),:)) + ...
    bsxfun(@times,  t23(ti31), V(F(F31,3),:)) ...
    ];

  % I is just concatenation of each I12
  I = [I12;I23;I31];

end


function [w] = projectSimplex(v)
% Computest the minimum L2-distance projection of vector v onto the probability simplex
nVars = length(v);
mu = sort(v,'descend');
sm = 0;
for j = 1:nVars
    sm = sm+mu(j);
   if mu(j) - (1/j)*(sm-1) > 0
       row = j;
       sm_row = sm;
   end
end
theta = (1/row)*(sm_row-1);
w = max(v-theta,0);


function [C,T]=hungarian(A)
% HUNGARIAN - Solve the assignment problem using the Hungarian method.
%
% Usage: >> [C,T]=hungarian(A)
%
% Inputs:
%   A - a square (correlation/distance/cost) matrix.
%   C - the optimal assignment (closest row/col pairs)
%   T - the total (minimized) cost of the optimal assignment.
%
% Author: Niclas Borlin, 1996

% Adapted from the FORTRAN IV code in Carpaneto and Toth, "Algorithm 548:
% Solution of the assignment problem [H]", ACM Transactions on
% Mathematical Software, 6(1):104-111, 1980.
%
% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.
%                 Department of Computing Science, Ume University,
%                 Sweden.
%
% NOTE: A substantial effort was put into this code. If you use it for a
% publication or otherwise, please include an acknowledgement and notify
% me by email. /Niclas

[m,n]=size(A);

if (m~=n)
    error('HUNGARIAN: Cost matrix must be square!');
end

% Save original cost matrix.
orig=A;

% Reduce matrix.
A=hminired(A);

% Do an initial assignment.
[A,C,U]=hminiass(A);

% Repeat while we have unassigned rows.
while (U(n+1))
    % Start with no path, no unchecked zeros, and no unexplored rows.
    LR=zeros(1,n);
    LC=zeros(1,n);
    CH=zeros(1,n);
    RH=[zeros(1,n) -1];

    % No labelled columns.
    SLC=[];

    % Start path in first unassigned row.
    r=U(n+1);
    % Mark row with end-of-path label.
    LR(r)=-1;
    % Insert row first in labelled row set.
    SLR=r;

    % Repeat until we manage to find an assignable zero.
    while (1)
        % If there are free zeros in row r
        if (A(r,n+1)~=0)
            % ...get column of first free zero.
            l=-A(r,n+1);

            % If there are more free zeros in row r and row r in not
            % yet marked as unexplored..
            if (A(r,l)~=0 && RH(r)==0)
                % Insert row r first in unexplored list.
                RH(r)=RH(n+1);
                RH(n+1)=r;

                % Mark in which column the next unexplored zero in this row
                % is.
                CH(r)=-A(r,l);
            end
        else
            % If all rows are explored..
            if (RH(n+1)<=0)
                % Reduce matrix.
                [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR);
            end

            % Re-start with first unexplored row.
            r=RH(n+1);
            % Get column of next free zero in row r.
            l=CH(r);
            % Advance "column of next free zero".
            CH(r)=-A(r,l);
            % If this zero is last in the list..
            if (A(r,l)==0)
                % ...remove row r from unexplored list.
                RH(n+1)=RH(r);
                RH(r)=0;
            end
        end

        % While the column l is labelled, i.e. in path.
        while (LC(l)~=0)
            % If row r is explored..
            if (RH(r)==0)
                % If all rows are explored..
                if (RH(n+1)<=0)
                    % Reduce cost matrix.
                    [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR);
                end

                % Re-start with first unexplored row.
                r=RH(n+1);
            end

            % Get column of next free zero in row r.
            l=CH(r);

            % Advance "column of next free zero".
            CH(r)=-A(r,l);

            % If this zero is last in list..
            if(A(r,l)==0)
                % ...remove row r from unexplored list.
                RH(n+1)=RH(r);
                RH(r)=0;
            end
        end

        % If the column found is unassigned..
        if (C(l)==0)
            % Flip all zeros along the path in LR,LC.
            [A,C,U]=hmflip(A,C,LC,LR,U,l,r);
            % ...and exit to continue with next unassigned row.
            break;
        else
            % ...else add zero to path.

            % Label column l with row r.
            LC(l)=r;

            % Add l to the set of labelled columns.
            SLC=[SLC l];

            % Continue with the row assigned to column l.
            r=C(l);

            % Label row r with column l.
            LR(r)=l;

            % Add r to the set of labelled rows.
            SLR=[SLR r];
        end
    end
end

% Calculate the total cost.
T=sum(orig(logical(sparse(C,1:size(orig,2),1))));

function A=hminired(A)
%HMINIRED Initial reduction of cost matrix for the Hungarian method.
%
%B=assredin(A)
%A - the unreduced cost matris.
%B - the reduced cost matrix with linked zeros in each row.

% v1.0  96-06-13. Niclas Borlin, niclas@cs.umu.se.

[m,n]=size(A);

% Subtract column-minimum values from each column.
colMin=min(A);
A=A-colMin(ones(n,1),:);

% Subtract row-minimum values from each row.
rowMin=min(A')';
A=A-rowMin(:,ones(1,n));

% Get positions of all zeros.
[i,j]=find(A==0);

% Extend A to give room for row zero list header column.
A(1,n+1)=0;
for k=1:n
    % Get all column in this row.
    cols=j(k==i)';
    % Insert pointers in matrix.
    A(k,[n+1 cols])=[-cols 0];
end

function [A,C,U]=hminiass(A)
%HMINIASS Initial assignment of the Hungarian method.
%
%[B,C,U]=hminiass(A)
%A - the reduced cost matrix.
%B - the reduced cost matrix, with assigned zeros removed from lists.
%C - a vector. C(J)=I means row I is assigned to column J,
%              i.e. there is an assigned zero in position I,J.
%U - a vector with a linked list of unassigned rows.

% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.

[n,np1]=size(A);

% Initialize return vectors.
C=zeros(1,n);
U=zeros(1,n+1);

% Initialize last/next zero "pointers".
LZ=zeros(1,n);
NZ=zeros(1,n);

for i=1:n
    % Set j to first unassigned zero in row i.
	lj=n+1;
	j=-A(i,lj);

    % Repeat until we have no more zeros (j==0) or we find a zero
	% in an unassigned column (c(j)==0).

	while (C(j)~=0)
		% Advance lj and j in zero list.
		lj=j;
		j=-A(i,lj);

		% Stop if we hit end of list.
		if (j==0)
			break;
		end
	end

	if (j~=0)
		% We found a zero in an unassigned column.

		% Assign row i to column j.
		C(j)=i;

		% Remove A(i,j) from unassigned zero list.
		A(i,lj)=A(i,j);

		% Update next/last unassigned zero pointers.
		NZ(i)=-A(i,j);
		LZ(i)=lj;

		% Indicate A(i,j) is an assigned zero.
		A(i,j)=0;
	else
		% We found no zero in an unassigned column.

		% Check all zeros in this row.

		lj=n+1;
		j=-A(i,lj);

		% Check all zeros in this row for a suitable zero in another row.
		while (j~=0)
			% Check the in the row assigned to this column.
			r=C(j);

			% Pick up last/next pointers.
			lm=LZ(r);
			m=NZ(r);

			% Check all unchecked zeros in free list of this row.
			while (m~=0)
				% Stop if we find an unassigned column.
				if (C(m)==0)
					break;
				end

				% Advance one step in list.
				lm=m;
				m=-A(r,lm);
			end

			if (m==0)
				% We failed on row r. Continue with next zero on row i.
				lj=j;
				j=-A(i,lj);
			else
				% We found a zero in an unassigned column.

				% Replace zero at (r,m) in unassigned list with zero at (r,j)
				A(r,lm)=-j;
				A(r,j)=A(r,m);

				% Update last/next pointers in row r.
				NZ(r)=-A(r,m);
				LZ(r)=j;

				% Mark A(r,m) as an assigned zero in the matrix . . .
				A(r,m)=0;

				% ...and in the assignment vector.
				C(m)=r;

				% Remove A(i,j) from unassigned list.
				A(i,lj)=A(i,j);

				% Update last/next pointers in row r.
				NZ(i)=-A(i,j);
				LZ(i)=lj;

				% Mark A(r,m) as an assigned zero in the matrix . . .
				A(i,j)=0;

				% ...and in the assignment vector.
				C(j)=i;

				% Stop search.
				break;
			end
		end
	end
end

% Create vector with list of unassigned rows.

% Mark all rows have assignment.
r=zeros(1,n);
rows=C(C~=0);
r(rows)=rows;
empty=find(r==0);

% Create vector with linked list of unassigned rows.
U=zeros(1,n+1);
U([n+1 empty])=[empty 0];

function [A,C,U]=hmflip(A,C,LC,LR,U,l,r)
%HMFLIP Flip assignment state of all zeros along a path.
%
%[A,C,U]=hmflip(A,C,LC,LR,U,l,r)
%Input:
%A   - the cost matrix.
%C   - the assignment vector.
%LC  - the column label vector.
%LR  - the row label vector.
%U   - the
%r,l - position of last zero in path.
%Output:
%A   - updated cost matrix.
%C   - updated assignment vector.
%U   - updated unassigned row list vector.

% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.

n=size(A,1);

while (1)
    % Move assignment in column l to row r.
    C(l)=r;

    % Find zero to be removed from zero list..

    % Find zero before this.
    m=find(A(r,:)==-l);

    % Link past this zero.
    A(r,m)=A(r,l);

    A(r,l)=0;

    % If this was the first zero of the path..
    if (LR(r)<0)
        % remove row from unassigned row list and return.
        U(n+1)=U(r);
        U(r)=0;
        return;
    else

        % Move back in this row along the path and get column of next zero.
        l=LR(r);

        % Insert zero at (r,l) first in zero list.
        A(r,l)=A(r,n+1);
        A(r,n+1)=-l;

        % Continue back along the column to get row of next zero in path.
        r=LC(l);
    end
end

function [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR)
%HMREDUCE Reduce parts of cost matrix in the Hungerian method.
%
%[A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR)
%Input:
%A   - Cost matrix.
%CH  - vector of column of 'next zeros' in each row.
%RH  - vector with list of unexplored rows.
%LC  - column labels.
%RC  - row labels.
%SLC - set of column labels.
%SLR - set of row labels.
%
%Output:
%A   - Reduced cost matrix.
%CH  - Updated vector of 'next zeros' in each row.
%RH  - Updated vector of unexplored rows.

% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.

n=size(A,1);

% Find which rows are covered, i.e. unlabelled.
coveredRows=LR==0;

% Find which columns are covered, i.e. labelled.
coveredCols=LC~=0;

r=find(~coveredRows);
c=find(~coveredCols);

% Get minimum of uncovered elements.
m=min(min(A(r,c)));

% Subtract minimum from all uncovered elements.
A(r,c)=A(r,c)-m;

% Check all uncovered columns..
for j=c
    % ...and uncovered rows in path order..
    for i=SLR
        % If this is a (new) zero..
        if (A(i,j)==0)
            % If the row is not in unexplored list..
            if (RH(i)==0)
                % ...insert it first in unexplored list.
                RH(i)=RH(n+1);
                RH(n+1)=i;
                % Mark this zero as "next free" in this row.
                CH(i)=j;
            end
            % Find last unassigned zero on row I.
            row=A(i,:);
            colsInList=-row(row<0);
            if (length(colsInList)==0)
                % No zeros in the list.
                l=n+1;
            else
                l=colsInList(row(colsInList)==0);
            end
            % Append this zero to end of list.
            A(i,l)=-j;
        end
    end
end

% Add minimum to all doubly covered elements.
r=find(coveredRows);
c=find(coveredCols);

% Take care of the zeros we will remove.
[i,j]=find(A(r,c)<=0);

i=r(i);
j=c(j);

for k=1:length(i)
    % Find zero before this in this row.
    lj=find(A(i(k),:)==-j(k));
    % Link past it.
    A(i(k),lj)=A(i(k),j(k));
    % Mark it as assigned.
    A(i(k),j(k))=0;
end

A(r,c)=A(r,c)+m;


function [ vX, mX ] = SolveLsL1Admm( vX, mA, vY, paramLambda, sSolverParams )
% ----------------------------------------------------------------------------------------------- %
%[ vX ] = SolveProxTvAdmm( vY, mD, paramLambda, numIterations )
% Solves the L1 Norm Regualrized Least Squares using Alternating
% Direction Method of Multipliers (ADMM) Method.
% Basically solves the problem given by:
% $$ \arg \min_{ x \in \mathbb{R}^{n} } \frac{1}{2} {\left\| A x - y \right|}_{2}^{2} + \lambda {\left\| x \right\|}_{1} $$
% Input:
%   - vX                -   Optimization Vector.
%                           The vector to be optimized. Initialization of
%                           the iterative process.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - mA                -   Input Matirx.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vY                -   Measurements 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).
%   - paramRho          -   The Rho Parameter.
%                           Sets the weight of the equality constraint.
%                           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 ADMM - https://en.wikipedia.org/wiki/Augmented_Lagrangian_method#Alternating_direction_method_of_multipliers.
% Remarks:
%   1.  Using vanilla ADMM with no optimization of the parameter or
%       smoothing.
%   2.  For high values of `paramLambda` it will require many iterations.
% Known Issues:
%   1.  C
% TODO:
%   1.  D
% Release Notes:
%   -   1.0.000     05/08/2021  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

arguments
    vX (:, 1) {mustBeFloat, mustBeReal}
    mA (:, :) {mustBeFloat, mustBeReal}
    vY (:, 1) {mustBeFloat, mustBeReal}
    paramLambda (1, 1) {mustBeFloat, mustBeReal, mustBePositive}
    sSolverParams.paramRho (1, 1) {mustBeNumeric, mustBeReal, mustBePositive} = 5
    sSolverParams.numIterations (1, 1) {mustBeNumeric, mustBeReal, mustBePositive, mustBeInteger} = 100
end

MAT_TYPE_SKINNY = 1;
MAT_TYPE_FAT    = 2;

if(size(mA, 1) >= size(mA, 2))
    matType = MAT_TYPE_SKINNY;
else
    matType = MAT_TYPE_FAT;
end

paramRho        = sSolverParams.paramRho;
numIterations   = sSolverParams.numIterations;

mX = zeros(size(mA, 2), numIterations);

% Caching facotirzation
mC = MatrixFactorize(mA, paramRho, matType);
vAy = mA.' * vY;

vZ = ProxL1(vX, paramLambda / paramRho);
vU = vX - vZ;

mX(:, 1) = vX;

for ii = 2:numIterations

    vQ = vAy + (paramRho * (vZ - vU));

    % Matrix Inversion Lemma
    switch(matType)
        case(MAT_TYPE_SKINNY)
            vX = mC \ vQ;
        case(MAT_TYPE_FAT)
            vX = (vQ / paramRho) - ((mA.' * (mC \ (mA * vQ))) / (paramRho * paramRho));
    end

    vZ = ProxL1(vX + vU, paramLambda / paramRho);
    vU = vU + vX - vZ;

    mX(:, ii) = vX;

end

end

function [ mC ] = MatrixFactorize( mA, paramRho, matType )

MAT_TYPE_SKINNY = 1;
MAT_TYPE_FAT    = 2;

switch(matType)
    case(MAT_TYPE_SKINNY)
        mI = speye(size(mA, 2));
        mC = decomposition((mA.' * mA) + (paramRho * mI), 'chol');
    case(MAT_TYPE_FAT)
        mI = speye(size(mA, 1));
        mC = decomposition(mI + ((1 / paramRho) * (mA * mA.')), 'chol');
end

end

function [ vX ] = ProxL1( vX, lambdaFactor )

% Soft Thresholding
vX = max(vX - lambdaFactor, 0) + min(vX + lambdaFactor, 0);

end


%DELTA2TR Convert differential motion  to SE(3) homogeneous transform
%
% T = DELTA2TR(D) is a homogeneous transform (4x4) representing differential
% motion D (6x1).
%
% The vector D=(dx, dy, dz, dRx, dRy, dRz) represents infinitessimal translation
% and rotation, and is an approximation to the instantaneous spatial velocity
% multiplied by time step.
%
% Reference::
% - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.
%
% See also tr2delta, SE3.delta.

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

function delta = delta2tr(d)
    d = d(:);
    delta = eye(4,4) + [skew(d(4:6)) d(1:3); 0 0 0 0];


function X = PQRFFT (x, N, ifn)
% Calculate the DFT of a real N-point sequence or the inverse
% DFT corresponding to a real N-point sequence.
% ifn > 0, forward transform
%          input x(n)  - N real values
%          output X(k) - The first N/2+1 points are the real
%            parts of the transform, the next N/2-1 points
%            are the imaginary parts of the transform. However
%            the imaginary part for the first point and the
%            middle point which are known to be zero are not
%            stored.
% ifn < 0, inverse transform
%          input X(k) - The first N/2+1 points are the real
%            parts of the transform, the next N/2-1 points
%            are the imaginary parts of the transform. However
%            the imaginary part for the first point and the
%            middle point which are known to be zero are not
%            stored.
%          output x(n) - N real values

% P. Kabal $Revision: 1.1 $  $Date: 2003/12/07 13:34:11 $

if (ifn > 0)
    X = fft (x, N);
    XR = real(X(0+1:N/2+1));
    XI = imag(X(1+1:N/2-1+1));
    X = [XR XI];
else
    xR = [x(0+1:N/2+1)];
    xI = [0 x(N/2+1+1:N-1+1) 0];
    x = complex ([xR xR(N/2-1+1:-1:1+1)], [xI -xI(N/2-1+1:-1:1+1)]);
    X = real (ifft (x, N));
end


%% this is an implementation of kalman filter using belief propagation.
% author: Shuang Wang
% email: shw070@ucsd.edu
% Division of Biomedical Informatics, University of California, San Diego.

clear;
addpath(genpath('.'));
%%
% x_k = A x_t-1 + q; N(0, V_Q);
% y_k = B x_k + w; N(0, V_W);
% x_1 ~ N(mu_0, V_0);

%% generating fake data.
s = RandStream('mt19937ar','Seed',4);
RandStream.setGlobalStream(s);
T = 40;
x = zeros(2, T);
y = zeros(2, T);
d_y = size(y, 1);
d_x = size(x, 1);
mu_0 = [4; 4];
V_0 = [1, 0
       0, 1];
V_Q = [0.1,   0
         0, 0.3];
V_W = [0.05,   0
          0, 0.1];
A = [   1, 0.04
     0.02,    1];
B = [1, 0
     0, 1];

 x(:, 1) = mvnrnd(mu_0, V_0, 1)';
 y(:, 1) = B * x(:, 1) + mvnrnd([0, 0], V_W, 1)';
 for t = 2:T
     x(:, t) = A * x(:, t-1) + mvnrnd([0, 0], V_Q, 1)';
     y(:, t) = B * x(:, t) + mvnrnd([0, 0], V_W, 1)';
 end

[msg, varNode_x, factorNode_x, factorNode_y, belief] = initializeMessage(T, mu_0, V_0, d_x, y, B, V_W);

for bp_iter = 1:2*T
    %% variable node update
    for i = 1:T
        if (i < T)
            msg{varNode_x{i}.backwardNeighborMsgID}.toFactorNode ...
                = GaussianMultiply(msg{varNode_x{i}.forwardNeighborMsgID}.toVarNode,...
                msg{varNode_x{i}.lowerNeighborMsgID}.toVarNode);

            msg{varNode_x{i}.forwardNeighborMsgID}.toFactorNode ...
                = GaussianMultiply(msg{varNode_x{i}.backwardNeighborMsgID}.toVarNode,...
                msg{varNode_x{i}.lowerNeighborMsgID}.toVarNode);
        else
            msg{varNode_x{i}.backwardNeighborMsgID}.toFactorNode = msg{varNode_x{i}.lowerNeighborMsgID}.toVarNode;
        end
    end

    %% factor node update
    for i = 2:T
        msg{factorNode_x{i}.forwardNeighborMsgID}.toVarNode.mu ...
            = A * msg{factorNode_x{i}.backwardNeighborMsgID}.toFactorNode.mu;
        if(~isempty(msg{factorNode_x{i}.backwardNeighborMsgID}.toFactorNode.V))
            msg{factorNode_x{i}.forwardNeighborMsgID}.toVarNode.V ...
                = A * msg{factorNode_x{i}.backwardNeighborMsgID}.toFactorNode.V * (A') + V_Q;
            msg{factorNode_x{i}.forwardNeighborMsgID}.toVarNode.iV = [];
        else
            msg{factorNode_x{i}.forwardNeighborMsgID}.toVarNode.V = [];
            tmp_iAtiviA = (A')\msg{factorNode_x{i}.backwardNeighborMsgID}.toFactorNode.iV/A;
            msg{factorNode_x{i}.forwardNeighborMsgID}.toVarNode.iV ...
                = tmp_iAtiviA -(eye(d_x) + tmp_iAtiviA*V_Q)\tmp_iAtiviA*V_Q*tmp_iAtiviA;
        end
        msg{factorNode_x{i}.backwardNeighborMsgID}.toVarNode.mu ...
            = A\msg{factorNode_x{i}.forwardNeighborMsgID}.toFactorNode.mu;
        if(~isempty(msg{factorNode_x{i}.forwardNeighborMsgID}.toFactorNode.V))
            msg{factorNode_x{i}.backwardNeighborMsgID}.toVarNode.iV = [];
            msg{factorNode_x{i}.backwardNeighborMsgID}.toVarNode.V ...
                = A\(msg{factorNode_x{i}.forwardNeighborMsgID}.toFactorNode.V + V_Q)/(A');
        else
            tmp_iv = msg{factorNode_x{i}.forwardNeighborMsgID}.toFactorNode.iV;
            msg{factorNode_x{i}.backwardNeighborMsgID}.toVarNode.V = [];
            msg{factorNode_x{i}.backwardNeighborMsgID}.toVarNode.iV ...
                = (A')*(tmp_iv -(eye(d_x) + tmp_iv*V_Q)\tmp_iv*V_Q*tmp_iv)*A;
        end
    end
end % bp_iter

%% update belief
for i = 1:T
    belief{i} = GaussianMultiply(msg{varNode_x{i}.backwardNeighborMsgID}.toFactorNode,...
                msg{varNode_x{i}.backwardNeighborMsgID}.toVarNode);
    mu_tmp_sm(:,i) = belief{i}.mu;
end
%%
figure(1); clf;
plot(x(1, :), x(2, :), '.'); hold on;
plot(y(1, :), y(2, :), 'ro'); hold on;
plot(mu_tmp_sm(1, :), mu_tmp_sm(2, :), 'g*'); hold on;
h = legend('Hidden state', 'observation', 'kalman smoother');
set(h, 'box', 'off', 'location', 'best');
fprintf('SAD:x_z = %f\n', sum(abs((y(:) - x(:)))));
fprintf('SAD:kf_sm_z = %f\n', sum(abs((mu_tmp_sm(:) - x(:)))));


% AUTHOR :   CHANDAN KUMAR
% SUBJECT:   COMPUTER GRAPHICS AND SOLID MODELLING.
% TITLE  :   LINE CLIPPING ALGORITHM (CYRUS BECK )
% DATE   :   31/08/09
function lineclip()
clc, clear all;
x = input('Give coord[ x0 y0 x1 y1]: ');
v = input('Give Viewport coord[vx0 vy0 vx1 vy1]: ');
d = [(x(3)-x(1)) (x(4)-x(2)) -(x(3)-x(1))  -(x(4)-x(2))];
for i = 1:4
    if (i==1)||(i==3)
        j =1;
    else
        j =2;
    end
    t(i) = (v(i)-x(j))/d(j);
end
tl = min(t(1),t(3));
ti = max(t(2),t(4));

z(1) = x(1) +d(1)*ti ;   % x coord of clipped line
z(3) = x(1) +d(1)*tl ;   % end X coord of clipped line
z(2) = x(2) +d(2)*ti ;
z(4) = x(2) +d(2)*tl ;

plot([z(1) z(3)] ,[z(2) z(4)],'r-','LineWidth',2);   % Plots Clipped line
hold on,grid on
plot([v(1) v(1)] ,[v(2) v(4)],'b-','LineWidth',2);   % Plots Viewport
plot([x(1) z(3)],[x(2) z(4)],'k-');                  % Plots Unclipped line
legend('Clipped line','Viewport','Unclipped line',3)
plot([z(1) x(3)],[z(2) x(4)],'k-');                  % Plots Unclipped line
plot([v(1) v(3)] ,[v(2) v(2)],'b-','LineWidth',2);   % Plots Viewport
plot([v(3) v(3)] ,[v(2) v(4)],'b-','LineWidth',2);   % Plots Viewport
plot([v(1) v(3)] ,[v(4) v(4)],'b-','LineWidth',2);   % Plots Viewport
axis([v(1)-1 v(3)+1 v(2)-1 v(4)+1]);
title('Line clipping by Cyrus Beck algorithm')


function a = myiswt2(swc, lo_r, hi_r)
% ISWT2 Inverse discrete stationary wavelet transform 2-D.
% ISWT2 performs a multilevel 2-D stationary wavelet
% reconstruction using either a specific orthogonal wavelet
% ('wname', see WFILTERS for more information) or specific
% reconstruction filters (Lo_r and Hi_r).
%
% Filters lo_r and hi_r must be normalized by dividing by sqrt(2)
% (necessary for implmentation of Stationary Discrete WT)
%
% For X = MYISWT2(swc,Lo_r,Hi_r)
% swc contains H(1:N) V(1:N) D(1:N) A(N)
% lo_r is the reconstruction low-pass filter.
% hi_r is the reconstruction high-pass filter.
%
% See also IDWT2, SWT2, WAVEREC2.

% based on iswt2.m
% Sathish Ramani, October 11, 2009

%% Set DWT_Mode to 'per'.
old_modeDWT = dwtmode('status','nodisp');
modeDWT = 'per';
dwtmode(modeDWT,'nodisp');

%% Load coefficients
n = (size(swc, 3)-1)/3; %% Number of levels
h = swc(:,:,1:n);
v = swc(:,:,n+1:2*n);
d = swc(:,:,2*n+1:3*n);
a = swc(:,:,3*n+1);

[rx, cx, dump] = size(h);
for k = n:-1:1
	step = 2^(k-1);
	last = step;
	for first1 = 1:last
		iRow = first1:step:rx;
		lR   = length(iRow);
		for first2 = 1:last
			iCol = first2:step:cx;
			lC   = length(iCol);
			sR   = iRow;
			sC   = iCol;
			a(iRow,iCol) = idwt2LOC(a(sR,sC), h(sR,sC,k), v(sR,sC,k), d(sR,sC,k), ...
				lo_r, hi_r, [lR lC]);
		end
	end
end

% Restore DWT_Mode.
dwtmode(old_modeDWT, 'nodisp');

%===============================================================%
% INTERNAL FUNCTIONS
%===============================================================%
function y = idwt2LOC(a, h, v, d, lo_r, hi_r, sy)

y = upconvLOC(a,lo_r,lo_r,sy) ... % Approximation
	+ upconvLOC(h,hi_r,lo_r,sy) ... % Horizontal Detail
	+ upconvLOC(v,lo_r,hi_r,sy) ... % Vertical Detail
	+ upconvLOC(d,hi_r,hi_r,sy); % Diagonal Detail

%---------------------------------------------------------------%
function y = upconvLOC(x, f1, f2, s)

lf = length(f1);
% y  = dyadup(x,'mat',0,1);
y = x;
y  = wextend('2D', 'per', y, [lf/2,lf/2]);
y  = wconv2('col', y, f1);
y  = wconv2('row', y, f2);
y  = wkeep2(y, s, [lf lf]);
%===============================================================%


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function pathS = MC_BS(S0,r,d,T,sigma,NTime,NSim,NBatches)
%
% Path discretization in a Black-Scholes model
%
%   S0 spot price
%   K the strike price
%   r the riskless rate
%   d the dividend yield
%   sigma the volatility
%   T the maturity
%   NTime number of timesteps
%   NSim number of simulations
%   NBatches number of batches

Delta = T/NTime;                          % The discretisation step

pathS = zeros(NSim,NTime+1,NBatches);
lnS1 = zeros(NSim,NTime+1);               % init the logspot price path
lnS1(:,1)=log(S0*exp(-d*T));              % adjust due to dividend yield

omega = (r-d-0.5*sigma^2) * Delta;

for l = 1:NBatches
    dW = randn(NSim,NTime);               % precompute all randoms

    for i=2:NTime+1
        lnS1(:,i) = lnS1(:,i-1) + omega + sigma* sqrt(Delta)*dW(:,i-1);
    end
    pathS(:,:,l) = exp(lnS1);
end


function npartitions = partn_enum ( n, nmax )

%*****************************************************************************80
%
%% PARTN_ENUM enumerates the partitions of N with maximum element NMAX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the integer to be partitioned.
%    Normally N must be positive, but for this routine any
%    N is allowed.
%
%    Input, integer NMAX, the maximum element in the partition.
%    Normally, 1 <= NMAX <= N is required,
%    but for this routine any value of NMAX is allowed.
%
%    Output, integer NPARTITIONS is the number of partitions of N
%    with maximum element NMAX.
%
  if ( n <= 0 )

    npartitions = 0;

  elseif ( nmax <= 0 || n < nmax )

    npartitions = 0;

  else

    offset = 1;

    p = npart_table ( n, nmax );

    npartitions = p(n+offset,nmax+offset);

  end

  return
end


function value = sphere_unit_monomial_nd ( n, p )

%*****************************************************************************80
%
%% SPHERE_UNIT_MONOMIAL_ND integrates a monomial on a unit sphere in ND.
%
%  Integration region:
%
%    Points X(1:N) such that
%
%      Sum ( X(1:N)**2 ) == 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gerald Folland,
%    How to Integrate a Polynomial Over a Sphere,
%    American Mathematical Monthly,
%    Volume 108, May 2001, pages 446-448.
%
%  Parameters:
%
%    Input, integer N, the dimension of the space.
%
%    Input, integer P(N), the exponents of X(1) through X(N) in the monomial.
%    The exponents P(N) must be nonnegative.
%
%    Output, real SPHERE_UNIT_MONOMIAL_ND, the integral of
%    X1**P(1)*X2**P(2)*...*XN**P(N) over the unit sphere.
%
  for i = 1 : n
    if ( mod ( p(i), 2 ) == 1 )
      value = 0.0E+00;
      return
    end
  end

  temp = 0.0E+00;
  arg2 = 0.0E+00;

  for i = 1 : n
    arg1 = ( p(i) + 1 ) / 2.0E+00;
    temp = temp + gammaln ( arg1 );
    arg2 = arg2 + arg1;
  end
  temp = temp - gammaln ( arg2 );

  value = 2.0E+00 * exp ( temp );

  return
end


%% Machine Learning Online Class - Exercise 1: Linear Regression

%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  linear exercise. You will need to complete the following functions
%  in this exericse:
%
%     warmUpExercise.m
%     plotData.m
%     gradientDescent.m
%     computeCost.m
%     gradientDescentMulti.m
%     computeCostMulti.m
%     featureNormalize.m
%     normalEqn.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%
% x refers to the population size in 10,000s
% y refers to the profit in $10,000s
%

%% Initialization
clear ; close all; clc

%% ==================== Part 1: Basic Function ====================
% Complete warmUpExercise.m
fprintf('Running warmUpExercise ... \n');
fprintf('5x5 Identity Matrix: \n');
warmUpExercise()

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

%% ======================= Part 2: Plotting =======================
fprintf('Plotting Data ...\n')
data = load('ex1data1.txt');
X = data(:, 1); y = data(:, 2);
m = length(y); % number of training examples

% Plot Data
% Note: You have to complete the code in plotData.m
plotData(X, y);

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

%% =================== Part 3: Gradient descent ===================
fprintf('Running Gradient Descent ...\n')

X = [ones(m, 1), data(:,1)]; % Add a column of ones to x
theta = zeros(2, 1); % initialize fitting parameters

% Some gradient descent settings
iterations = 1500;
alpha = 0.01;

% compute and display initial cost
computeCost(X, y, theta)

% run gradient descent
theta = gradientDescent(X, y, theta, alpha, iterations);

% print theta to screen
fprintf('Theta found by gradient descent: ');
fprintf('%f %f \n', theta(1), theta(2));

% Plot the linear fit
hold on; % keep previous plot visible
plot(X(:,2), X*theta, '-')
legend('Training data', 'Linear regression')
hold off % don't overlay any more plots on this figure

% Predict values for population sizes of 35,000 and 70,000
predict1 = [1, 3.5] *theta;
fprintf('For population = 35,000, we predict a profit of %f\n',...
    predict1*10000);
predict2 = [1, 7] * theta;
fprintf('For population = 70,000, we predict a profit of %f\n',...
    predict2*10000);

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

%% ============= Part 4: Visualizing J(theta_0, theta_1) =============
fprintf('Visualizing J(theta_0, theta_1) ...\n')

% Grid over which we will calculate J
theta0_vals = linspace(-10, 10, 100);
theta1_vals = linspace(-1, 4, 100);

% initialize J_vals to a matrix of 0's
J_vals = zeros(length(theta0_vals), length(theta1_vals));

% Fill out J_vals
for i = 1:length(theta0_vals)
    for j = 1:length(theta1_vals)
	  t = [theta0_vals(i); theta1_vals(j)];
	  J_vals(i,j) = computeCost(X, y, t);
    end
end

% Because of the way meshgrids work in the surf command, we need to
% transpose J_vals before calling surf, or else the axes will be flipped
J_vals = J_vals';
% Surface plot
figure;
surf(theta0_vals, theta1_vals, J_vals)
xlabel('\theta_0'); ylabel('\theta_1');

% Contour plot
figure;
% Plot J_vals as 15 contours spaced logarithmically between 0.01 and 100
contour(theta0_vals, theta1_vals, J_vals, logspace(-2, 3, 20))
xlabel('\theta_0'); ylabel('\theta_1');
hold on;
plot(theta(1), theta(2), 'rx', 'MarkerSize', 10, 'LineWidth', 2);


function p = predictOneVsAll(all_theta, X)
%PREDICT Predict the label for a trained one-vs-all classifier. The labels
%are in the range 1..K, where K = size(all_theta, 1).
%  p = PREDICTONEVSALL(all_theta, X) will return a vector of predictions
%  for each example in the matrix X. Note that X contains the examples in
%  rows. all_theta is a matrix where the i-th row is a trained logistic
%  regression theta vector for the i-th class. You should set p to a vector
%  of values from 1..K (e.g., p = [1; 3; 1; 2] predicts classes 1, 3, 1, 2
%  for 4 examples)

m = size(X, 1);
num_labels = size(all_theta, 1);

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

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters (one-vs-all).
%               You should set p to a vector of predictions (from 1 to
%               num_labels).
%
% Hint: This code can be done all vectorized using the max function.
%       In particular, the max function can also return the index of the
%       max element, for more information see 'help max'. If your examples
%       are in rows, then, you can use max(A, [], 2) to obtain the max
%       for each row.
%

for c = 1:m
	one_example = X(c,:);
	[max_value, max_index] = max(all_theta * one_example');
	p(c,1) = max_index;

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

end


function gb=gabor_fn(bw,gamma,psi,lambda,theta)
% bw    = bandwidth, (1)
% gamma = aspect ratio, (0.5)
% psi   = phase shift, (0)
% lambda= wave length, (>=2)
% theta = angle in rad, [0 pi)

sigma = lambda/pi*sqrt(log(2)/2)*(2^bw+1)/(2^bw-1);
sigma_x = sigma;
sigma_y = sigma/gamma;

sz=fix(8*max(sigma_y,sigma_x));
if mod(sz,2)==0, sz=sz+1;end

% alternatively, use a fixed size
% sz = 60;

[x y]=meshgrid(-fix(sz/2):fix(sz/2),fix(sz/2):-1:fix(-sz/2));
% x (right +)
% y (up +)

% Rotation
x_theta=x*cos(theta)+y*sin(theta);
y_theta=-x*sin(theta)+y*cos(theta);

gb=exp(-0.5*(x_theta.^2/sigma_x^2+y_theta.^2/sigma_y^2)).*cos(2*pi/lambda*x_theta+psi);
imshow(gb/2+0.5);


function F = tempCompCurve(x,Tdata)
%% Calculate Temperature Curve given Resistor and Thermistor Values
% Copyright (c) 2012, MathWorks, Inc.
%% Input voltage
Vin = 1.1;

%% Thermistor Calculations
% Values in x: R1 R2 R3 R4 RTH1(T_25degc) Beta1 RTH2(T_25degc) Beta2
% Thermistors are represented by:
%   Room temperature is 25degc: T_25
%   Standard value is at 25degc: RTHx_25
%   RTHx is the thermistor resistance at various temperature
% RTH(T) = RTH(T_25degc) / exp (Beta * (T-T_25)/(T*T_25)))
T_25 = 298.15;
T_off = 273.15;
Beta1 = x(6);
Beta2 = x(8);
RTH1 = x(5) ./ exp(Beta1 * ((Tdata+T_off)-T_25)./((Tdata+T_off)*T_25));
RTH2 = x(7) ./ exp(Beta2 * ((Tdata+T_off)-T_25)./((Tdata+T_off)*T_25));

%% Define equivalent circuits for parallel R's and RTH's
R1_eq = x(1)*RTH1./(x(1)+RTH1);
R3_eq = x(3)*RTH2./(x(3)+RTH2);

%% Calculate voltages at Point B
F = Vin * (R3_eq + x(4))./(R1_eq + x(2) + R3_eq + x(4));


% Isoparametric Formulation Implementation
% clear memory
clear all

%% Physical Parameters
% numberElements: number of elements
numberElements = 6;

% Initial Area
A = 12.5e-4; % m^2

% Modulus of elasticity
E = 2e11; % N/m^2

% Length of bar
Ltotal = 1.5; % m

%% Exact solutions
xx = 0:0.1:1.5;
displacements_exact = (40000*xx.^3/3-45000)/(E*A);
stress_exact = (40000*xx.^2)/A;

figure(3)

% Displacements
subplot(1,2,1); hold on;
plot(xx,displacements_exact,'-r');

% Stress
subplot(1,2,2); hold on;

plot(xx,stress_exact,'-r');

test = [1 2 4 8];
for numberElements = test

% Build nodes coordinates and middle points,
delta_x = Ltotal/numberElements;
i = 0:numberElements;
x = delta_x*i';x_mid = (x(2:end)-x(1:end-1))/2 + x(1:end-1);

% Length of each element
L = x(2:end)-x(1:end-1);

%% Build Elements
% numberNodes: number of nodes
numberNodes = numberElements + 1;

% generation of coordinates and connectivities
elementNodes = zeros(numberElements,2);
elementNodes(1,:) = [1,2];
for i = 2:numberElements
    elementNodes(i,:) = elementNodes(i-1,:) + 1;
end
nodeCoordinates = x;

% for structure:
   % displacements: displacement vector
   % force : force vector
   % stiffness: stiffness matrix
force=zeros(numberNodes,1);
stiffness=zeros(numberNodes,numberNodes);
% computation of the system stiffness matrix and force vector
for e=1:numberElements;
  % elementDof: element degrees of freedom (Dof)
  elementDof=elementNodes(e,:) ;
  detJacobian=L(e)/2;
  invJacobian=1/detJacobian;
  ngp = 2;
  [w,xi]=gauss1d(ngp);
  xc=0.5*(nodeCoordinates(elementDof(1))+nodeCoordinates(elementDof(2)));
  for ip=1:ngp;
      [shape,naturalDerivatives]=shapeFunctionL2(xi(ip));
      B=naturalDerivatives*invJacobian;
      stiffness(elementDof,elementDof)=...
      stiffness(elementDof,elementDof)+ B'*B*w(ip)*detJacobian*E*A;
      force(elementDof) = force(elementDof)+...
          -80000*(xc+detJacobian*xi(ip))*shape'*detJacobian*w(ip);
  end
end

%% BCs and solution
% prescribed dofs
prescribedDof = find(nodeCoordinates == 1.5); % fixed at node x = 1.5 m
% solution
GDof=numberNodes;
displacements=solution(GDof,prescribedDof,stiffness,force);
% output displacements/reactions
outputDisplacementsReactions(displacements,stiffness, ...
    numberNodes,prescribedDof,force)

%% Stress
stress = zeros(numberElements,1);
for e=1:numberElements;
    stress(elementNodes(e,:)) = E*B*displacements(elementNodes(e,:));
end

%% plot figures

switch numberElements
    case 1
        line='r*--';
    case 2
        line='g*--';
    case 4
        line='k*--';
    case 8
        line='m*--';
end

% Displacements
subplot(1,2,1); plot(nodeCoordinates,displacements,line)

% Stress
subplot(1,2,2); stairs(nodeCoordinates,stress,line)

end %end for
subplot(1,2,1); legend('Exact','NEL=1','NEL=2','NEL=4','NEL=8',2); hold off
subplot(1,2,2); legend('Exact','NEL=1','NEL=2','NEL=4','NEL=8',2); hold off


%
% Implementation of Bertsekas' auction algorithm [1] for a very fast
% solution of the linear assignment problem, i.e. it is able
% to solve a 1000-by-1000 problem in ~1.5s.
%
% In practice, with respect to running time, the auction algorithm
% outperforms the Kuhn-Munkres (or Hungarian) algorithm significantly.
% The auction algorithm has average run-time complexity O(N^2*log(N)) and
% worst-case complexity O(N^3), whereas the Kuhn-Munkres algorithm has
% average- and worst-case complexity  O(N^3).
%
% Note that only global optima are found for integer-valued benefit matrices.
% For real-valued benefit matrices a scaling of the values needs to be applied
% by multiplication with a large number. This scaling factor depends on the
% desired accuracy, as the global solution is found for the integral part
% of the benefit matrix, whilst there is no guarantee that the fractional part
% of the benefits are properly taken into account. However, in practical cases
% it seems advantageous to not round the resulting benefit matrix and retain
% the fractional parts in the benefit matrix. Also note that larger scalings
% of the benefit matrix increase the run-time, so a problem-specific trade-off
% between runtime and accuracy must be chosen.
%
% Runtime can be slightly decreased by installing the FastSet library,
% available on
% http://www.levmuchnik.net/Content/ProgrammingTips/MatLab/FastSet/FastSet.html
%
% Input:
%               A                       N-by-N benefit matrix (higher
%                                       values indicate a better match)
%               [epsilon]               Initial value of epsilon (optional)
%               [epsilonDecreaseFactor] Decreas factor of epsilon
%                                       (optional)
%
% Output:
%               assignments             Resulting assignments
%               [prices]                Prices used during auctions
%                                       (optional)
%
% Example:      See the function test() below for a usage example.
%               Typically only the benefit matrix A is given as input and the
%               first output argument is relevant. epsilon and
%               epsilonDecreaseFactor can be used to heuristically adapt
%               runtime. The example below can also be used for testing
%               whether a sufficient amount of scaling has been performed.
%               This is done by solving the assignment problem using CVX
%               (available on http://cvxr.com/cvx/download/ ) and comparing
%               its result to the solution with the auction algorithm.
%
%
% [1]	Bertsekas, D.P. 1998. Network Optimization Continuous and Discrete Models.
%
% Implementation by Florian Bernard ( f.bernardpi [at] gmail [dot] com )
%
% Created on 25/07/2014

function [assignments, prices] = ...
    assignmentProblemAuctionAlgorithm(A, epsilon, epsilonDecreaseFactor)

N = size(A,1);
assignments = nan(N,1);
prices = ones(N,1);

% check which unique function we use
if ( exist('fast_unique', 'file') )
    % requires FastSet, available on
    % http://www.levmuchnik.net/Content/ProgrammingTips/MatLab/FastSet/FastSet.html
    uniqueFcn = @fast_unique;
else
    uniqueFcn = @unique;
end

% heuristic for setting epsilon
A = A*(N+1);
if ( ~exist('epsilon', 'var') || isempty(epsilon) )
    maxAbsA = max(abs(A(:)));
    epsilon = maxAbsA/25;
%     if ( sum(abs(A(:)) > 0.0001) > 0.7*N*N ) % non-sparse
%         ...
%     else
%         epsilon = 0.5*((N*maxVal)/5 + N*maxVal); % see page 260 in [1]
%     end
end

if ( ~exist('epsilonDecreaseFactor', 'var') )
    epsilonDecreaseFactor = 0.2;
end

while (epsilon >= 1)
    % the outer loop performs epsilon-scaling in order to speed up execution
    % time. In particular, an updated prices array is computed in each
    % round, which speeds up further rounds with lower values of epsilon.
    assignments = nan(N,1);
    while (any(isnan(assignments)))
    %% forward-auction algorithm
        %% bidding phase
        % find unassigned indices
        unassignedIdx = find(isnan(assignments));
        nUnassigned = numel(unassignedIdx);

        % find best and second best objects
        AijMinusPj = bsxfun(@minus, A(unassignedIdx,:), prices');

        [~,viIdx] = max(AijMinusPj, [], 2);

        for i=1:nUnassigned
            AijMinusPj(i,viIdx(i)) = -inf;
        end
        wi = max(AijMinusPj, [], 2);

        % compute bids
        bids = nan(nUnassigned,1);
        for i=1:nUnassigned
            bids(i) = A(unassignedIdx(i),viIdx(i)) - wi(i) + epsilon;
        end

        %% assignment phase
        objectsThatHaveBeenBiddedFor = uniqueFcn(viIdx);
        for uniqueObjIdx=1:numel(objectsThatHaveBeenBiddedFor)
            currObject = objectsThatHaveBeenBiddedFor(uniqueObjIdx);
            personssWhoGaveBidsForJ = find(viIdx==currObject);

            [prices(currObject), idx] = max(bids(personssWhoGaveBidsForJ));
            personWithHighestBid = unassignedIdx(personssWhoGaveBidsForJ(idx));

            % remove previous assignment and store new assignment (the person
            % with highest bid)
            assignments(assignments==currObject) = nan;
            assignments(personWithHighestBid) = currObject;

        end
    end
    epsilon = epsilon * epsilonDecreaseFactor; % refine epsilon
end
end

function test()
N = 1000;

X = rand(N,3);
reference = rand(N,3);

A = X*reference';
%% Apply Auction algorithm
tic
    % scale A such that the integer version of A has sufficient accuracy
    % if S1 and P are not equal, the scaling factor needs to be increased
    scalingFactor = 10^6;
    Ascaled = A*scalingFactor;

    [assignments] = assignmentProblemAuctionAlgorithm(Ascaled);

    % create permutation matrix from assignments
    linIdx = sub2ind(size(A),assignments',1:size(A,1));
    P = sparse(size(A,1),size(A,2));
    P(linIdx) = 1;
toc

%% Use CVX for checking correctness of result
%  (Useful in particular for non-integer matrices A)
%  Note that this is really slow as CVX needs to create the canonical form
%  of the LP before it can call Gurobi. Creating the canonical form by hand
%  and directly calling Gurobi is much faster.
E = ones(N,1);

tic
    cvx_begin % quiet
        cvx_solver gurobi % uncomment this line if Gurobi is not available
        variable S1(N,N) nonnegative;
        maximize (trace(S1*A));
        S1*E==E;
        E'*S1==E';
    cvx_end
toc

% check if results are equal
if ( sum(sum(full(S1)~=full(P))) == 0 )
    disp('Correct results using Auction algorithm');
else
    error('Incorrect results using Auction algorithm. You probably need to scale A by a larger factor!');
end
end


function [SNR] = bvpsnr(BVP, FS, HR, PlotTF)
%BVPSNR Estimates the signal-to-noise ratio of the blood volume pulse
% signal. Adapted from the method by G. de Haan, TBME, 2013.
% SNR calculated as the ratio (in dB) of power contained within +/- 0.1 Hz
% of the reference heart rate frequency and +/- 0.2 of its first
% harmonic and sum of all other power between 0.5 and 4 Hz.
% Adapted from the method by G. de Haan, TBME, 2013
%
%   Inputs:
%       BVP                     = A BVP timeseries.
%       FS                      = The sample rate of the BVP time series (Hz/fps).
%       HR                      = The reference heart rate (Hz/fps).
%       PlotTF                  = Boolean to turn plotting results on or off.
%
%   Outputs:
%       SNR                     = Blood Volume Pulse Signal-to-Noise Ratio.
%
% Daniel McDuff, Ethan Blackford, January 2019
% Copyright (c)
% Licensed under the MIT License and the RAIL AI License.

HR_F=HR/60;

NyquistF = FS/2;
FResBPM = 0.5; %resolution (bpm) of bins in power spectrum used to determine PR and SNR
N = (60*2*NyquistF)/FResBPM; %number of bins in power spectrum

%% Construct Periodogram
[Pxx,F] = periodogram(BVP,hamming(length(BVP)),N,FS);
GTMask1 = (F >= HR_F-0.1)&(F <= HR_F+0.1);
GTMask2 = (F >= HR_F*2-0.2)&(F <= HR_F*2+0.2);
SPower = sum(Pxx(GTMask1|GTMask2));
FMask2 = (F >= 0.5)&(F <= 4);
AllPower = sum(Pxx(FMask2));
SNR = pow2db(SPower/(AllPower-SPower));

%% Optionally plot the power spectrum and regions used to calculate the SNR
if(PlotTF)
    % Plot spower spectrum and SNR regions
    figure
    plot(F,pow2db(Pxx))
    title('Power Spectrum and SNR Regions')
    xlabel('Frequency (Hz)')
    ylabel('Power (dB)')
    xlim([0.5 4])
    ylimreg=ylim;
    hold on

    % HR peak
    %line([HR_F,HR_F],ylim,'Color','magenta','LineStyle','-.')

    % HR peak region
    line([HR_F-0.1,HR_F-0.1],ylim,'Color','red','LineStyle','--')
    line([HR_F+0.1,HR_F+0.1],ylim,'Color','red','LineStyle','--')

    % First harmonic
    line([HR_F*2-0.2,HR_F*2-0.2],ylim,'Color','red','LineStyle','--')
    line([HR_F*2+0.2,HR_F*2+0.2],ylim,'Color','red','LineStyle','--')

    % Overall power region
    line([0.5,0.5],ylim,'Color','black','LineStyle','-')
    line([4,4],ylim,'Color','black','LineStyle','-')

    % Adjust axes
    xlim([0 4.5])
    ylim(ylimreg);
end

end


function geometry_test1895 ( )

%*****************************************************************************80
%
%% TEST1895 tests SPHERE_UNIT_AREA_ND, SPHERE_UNIT_AREA_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2005
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST1895:\n' );
  fprintf ( 1, '  SPHERE_UNIT_AREA_ND evaluates the area of the unit\n' );
  fprintf ( 1, '  sphere in N dimensions.\n' );
  fprintf ( 1, '  SPHERE_UNIT_AREA_VALUES returns some test values.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     DIM_NUM    Exact          Computed\n' );
  fprintf ( 1, '             Area           Area\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, dim_num, area ] = sphere_unit_area_values ( n_data );

    if ( n_data == 0 )
      break
    end

    area2 = sphere_unit_area_nd ( dim_num );

    fprintf ( 1, '  %6d  %10f  %10f\n', dim_num, area, area2 );

  end

  return
end