% RES = RCONV2(MTX1, MTX2, CTR)
%
% Convolution of two matrices, with boundaries handled via reflection
% about the edge pixels.  Result will be of size of LARGER matrix.
%
% The origin of the smaller matrix is assumed to be its center.
% For even dimensions, the origin is determined by the CTR (optional)
% argument:
%      CTR   origin
%       0     DIM/2      (default)
%       1     (DIM/2)+1

% Eero Simoncelli, 6/96.

function c = rconv2(a,b,ctr)

if (exist('ctr') ~= 1)
  ctr = 0;
end

if (( size(a,1) >= size(b,1) ) & ( size(a,2) >= size(b,2) ))
    large = a; small = b;
elseif  (( size(a,1) <= size(b,1) ) & ( size(a,2) <= size(b,2) ))
    large = b; small = a;
else
  error('one arg must be larger than the other in both dimensions!');
end

ly = size(large,1);
lx = size(large,2);
sy = size(small,1);
sx = size(small,2);

%% These values are one less than the index of the small mtx that falls on
%% the border pixel of the large matrix when computing the first
%% convolution response sample:
sy2 = floor((sy+ctr-1)/2);
sx2 = floor((sx+ctr-1)/2);

% pad with reflected copies
clarge = [
    large(sy-sy2:-1:2,sx-sx2:-1:2), large(sy-sy2:-1:2,:), ...
	large(sy-sy2:-1:2,lx-1:-1:lx-sx2); ...
    large(:,sx-sx2:-1:2),    large,   large(:,lx-1:-1:lx-sx2); ...
    large(ly-1:-1:ly-sy2,sx-sx2:-1:2), ...
      large(ly-1:-1:ly-sy2,:), ...
      large(ly-1:-1:ly-sy2,lx-1:-1:lx-sx2) ];

c = conv2(clarge,small,'valid');


function [u,v,un,vn]=CalculateVelocityFromVortex2D(s,xc,yc,x,y)

%--------------------------------------------------------------------------
%CalculateVelocityFromVortex2D
%Version 1.00
%Created by Stepen
%Created 8 January 2011
%--------------------------------------------------------------------------
%CalculateVelocityFromVortex2D calculates the flow velocity influenced by
%several given vortex flow at several given locations in 2D domain.
%CalculateVelocityFromVortex2D assumes that the vortexs that influence the
%flow are point vortexes.
%--------------------------------------------------------------------------
%Syntax:
%[u,v,un,vn]=CalculateVelocityFromvortex2D(s,xc,yc,x,y)
%Input argument:
%- s (m x 1 num) specifies the strength of all point vortexes.
%- xc (m x 1 num) specifies the x axis location of all point vortexes.
%- yc (m x 1 num) specifies the y axis location of all point vortexes.
%- x (p x q num) specifies the x axis location of the corresponding
%  velocity vector.
%- y (p x q num) specifies the y axis location of the corresponding
%  velocity vector.
%Output argument:
%- u (m x n) specifies the horizontal velocity component at the
%  corresponding location.
%- u (m x n) specifies the vertical velocity component at the corresponding
%  location.
%- un (m x n num) specifies the normalized horizontal velocity component of
%  the corresponding location. un output was generated for plotting
%  purpose. Use this output instead of u for velocity quiver plotting.
%- vn (m x n num) specifies the normalized vertical velocity component of
%  the corresponding location. vn output was generated for plotting
%  purpose. Use this output instead of v for velocity quiver plotting.
%--------------------------------------------------------------------------

%CodeStart-----------------------------------------------------------------
%Checking input point vortexes
    s_size=size(s);
    xc_size=size(xc);
    yc_size=size(yc);
    if (s_size(2)~=1)||(xc_size(2)~=1)||(yc_size(2)~=1)
        error('Input point vortexs must be a i x 1 array!')
    end
    if (numel(s)~=numel(xc))||(numel(s)~=numel(yc))
        error('Input s, xc, and yc do not have the same size!')
    end
    vortexcount=numel(s);
%Checking input location
    x_size=size(x);
    y_size=size(y);
    if (x_size(1)~=y_size(1))||(x_size(2)~=y_size(2))
        error('Input x and y do not have the same size!')
    end
    rowcount=x_size(1);
    colcount=x_size(2);
%Preallocating array for speed
    u=zeros(rowcount,colcount);
    v=zeros(rowcount,colcount);
    un=zeros(rowcount,colcount);
    vn=zeros(rowcount,colcount);
%Calculating influenced velocity
    for i=1:1:rowcount
        for j=1:1:colcount
            for k=1:1:vortexcount
                if (x(i,j)==xc(k))&&(y(i,j)==yc(k))
                    u(i,j)=u(i,j);
                    v(i,j)=v(i,j);
                else
                    r=norm([(x(i,j)-xc(k)),(y(i,j)-yc(k))]);
                    vtheta=-s(k)/(2*pi()*r);
                    u(i,j)=u(i,j)+(vtheta*((y(i,j)-yc(k))/r));
                    v(i,j)=v(i,j)+(vtheta*((xc(k)-x(i,j))/r));
                end
            end
        end
    end
%Calculating normalized vector
    for i=1:1:rowcount
        for j=1:1:colcount
            V=norm([u(i,j),v(i,j)]);
            un(i,j)=u(i,j)/V;
            vn(i,j)=v(i,j)/V;
        end
    end
%CodeEnd-------------------------------------------------------------------

end


function f = p09_f ( n, x )

%*****************************************************************************80
%
%% P09_F evaluates the objective function for problem 9.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  ap = 0.00001;

  t1 = -1.0;
  t2 = 0.0;
  t3 = 0.0;
  d2 = 1.0;
  s2 = 0.0;

  for j = 1 : n
    t1 = t1 + ( n - j + 1 ) * x(j)^2;
    s1 = exp ( x(j) / 10.0 );
    if ( 1 < j )
      s3 = s1 + s2 - d2 * ( exp ( 0.1 ) + 1.0 );
      t2 = t2 + s3 * s3;
      t3 = t3 + ( s1 - 1.0 / exp ( 0.1 ) )^2;
    end
    s2 = s1;
    d2 = d2 * exp ( 0.1 );
  end

  f = ap * ( t2 + t3 ) + t1 * t1 + ( x(1) - 0.2 )^2;

  return
end


function neg_log_p = neglogsigmoid(log_odds)
% neg_log_p = NEGLOGSIGMOID(log_odds)
%   This is mathematically equivalent to -log(sigmoid(log_odds)),
%   but numerically better, when log_odds is large negative

assert(nargin==1)

neg_log_p = -log_odds;
e = exp(-log_odds);
f=find(e<e+1);
neg_log_p(f) = log(1+e(f));


classdef ProposedOptimalSuperEllipse < handle

    properties (Access = private)
       mxV
       myV
       qV
       alpha
       beta
       outputPath
    end

    methods (Access = public)

        function obj = ProposedOptimalSuperEllipse()
            obj.outputPath = '/home/alex/Dropbox/PaperStress/';
            obj.obtainOptimalSuperEllipseValues();
            obj.obtainOptimalCoeficientsForAnalyticalSuperEllipse();
            obj.plotAnalyticalSuperEllipse();
            obj.plotAnalyticalQvsMaxMxMy();
        end

        function obtainOptimalSuperEllipseValues(obj)
            superEllipse = PonderatedOptimalSuperEllipseComputer();
            superEllipse.compute();
            obj.mxV = superEllipse.mxV;
            obj.myV = superEllipse.myV;
            obj.qV  = superEllipse.qMean;
        end

        function obtainOptimalCoeficientsForAnalyticalSuperEllipse(obj)
            qAnalytical = @(alpha,beta) obj.analyticalExponent(alpha,beta,obj.mxV,obj.myV);
            options = optimoptions('fmincon','Display','iter');
            problem.options = options;
            problem.solver = 'fmincon';
            problem.objective = @(x) norm(qAnalytical(x(1),x(2)) - obj.qV)/norm(obj.qV);
            problem.x0 = [6,20];
            problem.ub = [50,50];
            problem.lb = [0,0];
            x = fmincon(problem);
            obj.alpha = x(1);
            obj.beta = x(2);
        end

        function plotAnalyticalSuperEllipse(obj)
            f = obj.plotExponent(obj.alpha,obj.beta);
            p = surfPrinter(f);
            xlabel('$m_1$','Interpreter','latex');
            ylabel('$m_2$','Interpreter','latex');
            zlabel('$q$','Interpreter','latex');
            fName = fullfile(obj.outputPath,'AnalyticalOptimalSuperEllipse');
            p.print(fName)
        end

        function plotAnalyticalQvsMaxMxMy(obj)
            mx = linspace(0.01,0.99,100);
            my = linspace(0.01,0.99,100);
            q = obj.analyticalExponent(obj.alpha,obj.beta,mx,my);
            f = figure();
            h{1} = plot(mx,q,'-+');
            xlabel('$\max(m_1,m_2)$','Interpreter','latex');
            ylabel('$q$','Interpreter','latex');
            fName = fullfile(obj.outputPath,'AnalyticalQvsMaxMxMy');
            p = plotPrinter(f,h);
            p.print(fName);
        end

        function f = plotExponent(obj,alpha,beta)
            f = figure();
            nMx = 100;
            nMy = 100;
            mxv = linspace(0.01,0.99,nMx);
            myv = linspace(0.01,0.99,nMy);
            q = zeros(nMx,nMy);
            for ix = 1:nMx
                for iy = 1:nMy
                    mx = mxv(ix);
                    my = myv(iy);
                    q(ix,iy) = obj.analyticalExponent(alpha,beta,mx,my);
                end
            end
            surf(mxv,myv,q')
        end

    end

    methods (Access = private, Static)

        function q = analyticalExponent(alpha,beta,m1,m2)
            qmin = 2;
            qmax = 32;
            q = max(qmin,min((1./(1-max(m1,m2).^beta)).^alpha,qmax));
        end

    end

end


function nleqs_master_ex2(alg)
if nargin < 1
    alg = 'DEFAULT';
end
x0 = [1; 2];
opt = struct( ...
    'verbose', 2, ...
    'alg', alg, ...
    'fd_opt', struct( ...
        'jac_approx_fcn', @jac_approx_fcn2, ...
        'labels', {{'P','Q'}}), ...
    'gs_opt', struct('x_update_fcn', @x_update_fcn2) );
[x, f, exitflag, output] = nleqs_master(@f2, x0, opt);
fprintf('\nexitflag = %d\n', exitflag);
fprintf('\nx = \n');
fprintf('   %2g\n', x);
fprintf('\nf = \n');
fprintf('   %12g\n', f);

function [f, J] = f2(x)
%% from Christi Patton Luks, https://www.youtube.com/watch?v=pJG4yhtgerg
f = [  x(1)^2 +   x(1)*x(2)   - 10;
       x(2)   + 3*x(1)*x(2)^2 - 57  ];
if nargout > 1
    J = [   2*x(1)+x(2)    x(1);
            3*x(2)^2       6*x(1)*x(2)+1    ];
end

function JJ = jac_approx_fcn2()
%% for use with fast-decoupled Newton's method
J = [7 2; 27 37];
JJ = {J(1,1), J(2,2)};

function x = x_update_fcn2(x, f)
%% for use with Gauss-Seidel method
x(1) = sqrt(10 - x(1)*x(2));
x(2) = sqrt((57-x(2))/3/x(1));


%  Figure 5.45 Feedback Control of Dynamic Systems, 5e
%                   Franklin, Powell, Emami
%
clf
numG=[1 0];
denG=[1 1 4];
K1=0:.1:8;   % K = 2 at breakin point
K2=100;
Kt=[K1 K2];
axis('square')
r=rlocus(numG,denG,Kt);    % roots vs. Kt
plot(r,'--'),grid on;
axis([-6 2 -4 4])
hold on
p=roots(denG);             % find and plot OL poles
z=roots(numG);             % find and plot OL zeros
plot(z(1),0,'o')
plot(real(p(1)),imag(p(1)),'x',real(p(2)),imag(p(2)),'x')
title('Fig. 5.45  Root Locus vs. K with K_{T} = 1')
xlabel('Re(s)')
ylabel('Im(s)')

% now do RL vs. K

Kt1 = 1;
r1=rlocus(numG,denG,Kt1);    % root at Kt = 1
plot(r1,'*')
num = 1;
den = poly(r1);
K = [0 30]
r2=rlocus(num,den,K);    % root locus vs. K
plot(r2,'-')

hold off
axis('normal')


function [A,S]=jade(X,m,Wprev)
% ICA implementation using JADE (used by the ICA function).
%
%   Inputs:
%       X               = Each column of X is a sample from the n sensors.
%       m               = Number of source signals (optional).
%       Wprev           = Timepoint at which to start process (default = 0 seconds).
%
%   Outputs:
%       A               = An n x m estimate of an unknown matrix.
%       S               = An m x T naive estimate of the source signals.
%
% Implementation by: JF Cardoso. (Version 1.5)
%
% Please Cite:
% Reference:
%  @article{CS_iee_94,
%   author = "Jean-Fran\c{c}ois Cardoso and Antoine Souloumiac",
%   journal = "IEE Proceedings-F",
%   title = "Blind beamforming for non {G}aussian signals",
%   number = "6",
%   volume = "140",
%   month = dec,
%   pages = {362-370},
%   year = "1993"}

% Determine the no of rows and colums of the observation matrix.
[n,T]	= size(X);
if nargin==1, m=n ; end;

nem     = m;                    % The nuumber of eigen-matrices to be diagonalized.
seuil	= 1/sqrt(T)/100;    % A statistical threshold for stopping the joint diag.

%% Whiten the Matrix:
% Assuming white noise:
if m<n,
 	[U,D]       = eig((X*X')/T);
	[pu,k]   = sort(diag(D));
 	ibl         = sqrt(pu(n-m+1:n)-mean(pu(1:n-m)));
 	bl          = ones(m,1) ./ ibl ;
 	W           = diag(bl)*U(1:n,k(n-m+1:n))';
 	IW          = U(1:n,k(n-m+1:n))*diag(ibl);
% Assuming no noise:
else
 	IW          = sqrtm((X*X')/T);
 	W           = inv(IW);
end;
Y	= W*X;

%% Cumulant Estimation:
R       = (Y*Y' )/T ;
C       = (Y*Y.')/T ;
Q       = zeros(m*m*m*m,1) ;
index	= 1;

for lx = 1:m ; Yl 	= Y(lx,:);
    for kx = 1:m ; Ykl 	= Yl.*conj(Y(kx,:));
        for jx = 1:m ; Yjkl	= Ykl.*conj(Y(jx,:));
            for ix = 1:m ;
                Q(index) = ...
                (Yjkl * Y(ix,:).')/T -  R(ix,jx)*R(lx,kx) -  R(ix,kx)*R(lx,jx) -  C(ix,lx)*conj(C(jx,kx))  ;
                index	= index + 1;
            end
        end
    end
end

%% Compute and Reshape the significant Eigen Matrices:
[U,D]	= eig(reshape(Q,m*m,m*m));
[la,K]	= sort(abs(diag(D)));

M	= zeros(m,nem*m);
Z	= zeros(m)	;
h	= m*m;
for u=1:m:nem*m,
	Z(:)            = U(:,K(h));
	M(:,u:u+m-1)	= la(h)*Z;
	h               = h-1;
end;

%% Approximate the Diagonalization of the Eigen Matrices:
B       = [ 1 0 0 ; 0 1 1 ; 0 -i i ] ;
Bt      = B' ;

encore	= 1;
if exist('Wprev','var'), V=inv(Wprev); else, V	= eye(m); end

%% Main Loop:
while encore, encore=0;
    for p = 1:m-1,
        for q = p+1:m,

            Ip = p:m:nem*m ;
            Iq = q:m:nem*m ;

            % Computing the Givens Angles:
            g	= [ M(p,Ip)-M(q,Iq)  ; M(p,Iq) ; M(q,Ip) ] ;
            [vcp,D] = eig(real(B*(g*g')*Bt));
            [la, K]	= sort(diag(D));
            angles	= vcp(:,K(3));
            if angles(1)<0 , angles= -angles ; end ;
            c	= sqrt(0.5+angles(1)/2);
            s	= 0.5*(angles(2)-j*angles(3))/c;

            % Update the matrices M and V by a Givens Rotation:
            if abs(s)>seuil,
                encore          = 1 ;
                pair            = [p;q] ;
                G               = [ c -conj(s) ; s c ] ;
                V(:,pair)       = V(:,pair)*G ;
                M(pair,:)       = G' * M(pair,:) ;
                M(:,[Ip Iq]) 	= [ c*M(:,Ip)+s*M(:,Iq) -conj(s)*M(:,Ip)+c*M(:,Iq) ] ;
            end
        end
    end
end

%% Estimation of the mixing matrix and signal separation
A	= IW*V;
S	= V'*Y ;

return ;


function [nll,g,H,T] = LogisticLoss(w,X,y)
% w(feature,1)
% X(instance,feature)
% y(instance,1)

[n,p] = size(X);

Xw = X*w;
yXw = y.*Xw;

nll = sum(mylogsumexp([zeros(n,1) -yXw]));

if nargout > 1
    if nargout > 2
        sig = 1./(1+exp(-yXw));
        g = -X.'*(y.*(1-sig));
    else
        %g = -X.'*(y./(1+exp(yXw)));
        g = -(X.'*(y./(1+exp(yXw))));
    end
end

if nargout > 2
    H = X.'*diag(sparse(sig.*(1-sig)))*X;
end

if nargout > 3
    T = zeros(p,p,p);
    for j1 = 1:p
        for j2 = 1:p
            for j3 = 1:p
                T(j1,j2,j3) = sum(y(:).^3.*X(:,j1).*X(:,j2).*X(:,j3).*sig.*(1-sig).*(1-2*sig));
            end
        end
    end
end


function L = statistical_filter(SpikeMat,sdd)
% This functions does statistical filtering on detected spikes by
% thresholding on waveform std and mean, and returns a logical matrix
% containing location of detected outliers (L).

Max_std = sdd.stat_filter.max_std;
Max_mean = sdd.stat_filter.max_mean;

spike_std = std(SpikeMat,0,2);
spike_mean = mean(SpikeMat,2);

% finding locations of threshold crossing points
L = (spike_std > Max_std) | (spike_mean > Max_mean) | (spike_mean < -Max_mean);

% check if perecent of detected outliers exceeds maximum allowed percent
if sum(L)/length(L) > sdd.stat_filter.max_percent_of_outliers
    % limiting number of detected outlier to maximum allowed percent by
    % sorting them based on criterion
    criterion = spike_std/std(spike_std) + abs(spike_mean)/std(spike_mean);
    criterion_s = sort(criterion,'descend');
    L = criterion > criterion_s(ceil(length(L)*sdd.stat_filter.max_percent_of_outliers));
end


function Qd = calculateQd(tau,alpha,h)
%calculates Qd for noise simulation algorithm from paper DISCRETE SIMULATION OF POWER LAW NOISE
%equation: Qd = h / (2*(2pi)^alpha * tau^(alpha-1))

Qd = h / (2*(2*pi)^alpha * tau^(alpha-1)); %calculate Qd


function [data,data2,Isymbols,Qsymbols]=serial_to_parallel(N)
%Bob Gess, June 2008 for EE 473
%Extracted and modified from original code (QPSK_TX_IQ_RX) written by JC and posted on
%Mathworks 12-23-2005.

%This module generates the data and splits it into the I channel and Q
%channel
%N=1e3;		        % Number of data bits(bit rate)
fs=40*2e3;		    % Sampling frequency
Fn=fs/2;            % Nyquist frequency
Ts=1/fs;	        % Sampling time = 1/fs
T=1/N;		        % Bit time
randn('state',0);   % Keeps PRBS from changing on reruns
td=[0:Ts:(N*T)-Ts]';% Time vector(data)(transpose)
%===================================================================
% Input data
%===================================================================
data=sign(randn(N,1))';%transpose
data1=ones(T/Ts,1)*data;
data2=data1(:);
%length(data2)

%display input data bits in command window
data_2=data2';
data_2=data_2 >0;
x=0;
transmitted_data_bits=data_2(1:(fs+x)/N:end);

%Serial to parallel (alternating)
tiq = [0:Ts*2:(N*T)-Ts]';% Time vector for I and Q symbols(transpose)

bs1=data(1:2:length(data));%odd
symbols=ones(T/Ts,1)*bs1;
Isymbols=symbols(:);%I_waveform
%LI=length(Isymbols)

bs2=data(2:2:length(data));%even
symbols1=ones(T/Ts,1)*bs2;
Qsymbols=symbols1(:);%Q_waveform
%LQ=length(Qsymbols)

%=====================================================================
%Plots
%======================================================================
figure(1)
subplot(3,1,1)
plot(td,data2)
axis([0 20/N -2 2]);
grid on
xlabel('                                                          Time')
ylabel('Amplitude')
title('Input Data')

subplot(3,1,2)
plot(tiq,Isymbols)
axis([0 20/N -2 2]);
grid on
xlabel('                                                          Time')
ylabel('Amplitude')
title('I Channel(one bit/symbol(phase)) Data')

subplot(3,1,3)
plot(tiq,Qsymbols)
axis([0 20/N -2 2]);
grid on
xlabel('                                                          Time')
ylabel('Amplitude')
title('Q Channel(one bit/symbol(phase)) Data')


% Lightspeed Toolbox.
% Efficient operations for Matlab programming.
% Version 2.1   10-Oct-2006
% By Tom Minka
% (c) Microsoft Corporation. All rights reserved.
%
% Matrix algebra
%   repmat           - Fast replacement for matlab's repmat.
%   xrepmat          - Matlab's original repmat.
%   row_sum          - Sum for each row.  Faster than 'sum'.
%   scale_rows       - Scale each row of a matrix.
%   scale_cols       - Scale each column of a matrix.
%   solve_triu       - Left division by upper triangular matrix.
%   solve_tril       - Left division by lower triangular matrix.
%   sqdist           - Squared Euclidean and Mahalanobis distance.
%   isposdef         - Check for positive-definiteness.
%   logdet           - log(determinant) for positive definite matrix.
%
% Statistics
%   normpdf          - Multivariate normal density.
%   normpdfln        - Log of multivariate normal density.
%   normcdf          - Normal cumulative distribution.
%   normcdfln        - Log of normal cumulative distribution.
%   normcdflogit     - Logit of normal cumulative distribution.
%   wishpdf          - Wishart probability density function.
%   wishpdfln        - Log of Wishart probability density function.
%   sample           - Sample from categorical distribution.
%   sample_vector    - Sample from multiple categorical distributions.
%   sample_hist      - Sample from multinomial distribution.
%   randbinom        - Sample from binomial distribution.
%   randnorm         - Sample from multivariate normal.
%   randgamma        - Sample from Gamma distribution.
%   randbeta         - Sample from Beta distribution.
%   randwishart      - Sample from Wishart distribution.
%   int_hist         - Histogram of integer values.
%
% Utility
%   logsumexp        - Sum in the log domain.
%   logmulexp        - Matrix multiply in the log domain.
%   ndgridmat        - Matrix of grid points.
%   ind2subv         - Subscript vector from linear index.
%   subv2ind         - Linear index from subscript vector.
%   gammaln          - Fast replacement for matlab's gammaln.
%   digamma          - Derivative of gammaln.
%   trigamma         - Derivative of digamma.
%   ndsum            - Sum over multiple dimensions.
%   ndmax            - Maximum over multiple dimensions.
%   ndlogsumexp      - Sum over multiple dimensions in the log domain.
%   maxdiff          - Maximum difference between structs or arrays.
%   makestruct       - Cell-friendly alternative to STRUCT.
%   setfields        - Set multiple fields of a structure.
%   struct2arglist   - Convert structure to cell array of fields/values.
%   sameobject       - Test if two variables correspond to the same object.
%   find_sameobject  - Find an object in a cell array.
%   toJava           - Convert to Java representation.
%   fromJava         - Convert from Java to Matlab.
%   glob             - Filename expansion via wildcards.
%   globstrings      - String matching via wildcards.
%
% Mutation
%   mutable          - Convert to a mutable object.
%   immutable        - Convert to an ordinary (immutable) object.
%
% Set operations
%   ismember_sorted  - True for member of sorted set.
%   match            - Location of matches in a set.
%   match_sorted     - Location of matches in a sorted set.
%   setdiff_sorted   - Set difference between sorted sets.
%   intersect_sorted - Set intersection between sorted sets.
%   union_sorted     - Set union of sorted sets.
%   union_sorted_rows - Set union of sorted sets of row vectors.
%   duplicated       - Find duplicated rows in a matrix.
%
% Readability
%   rows             - Number of rows.
%   cols             - Number of columns.
%   col_sum          - Sum for each column.
%   setdiag          - Modify the diagonals of a matrix.
%   finddiag         - Index of elements on diagonals.
%   argmax           - Index of maximum element.
%   argmin           - Index of minimum element.
%
% Flop counting
%   flops            - Read/write flop counter.
%   addflops         - Add to flop counter.
%   flops_chol       - Flops for Cholesky decomposition.
%   flops_col_sum    - Flops for column sums.
%   flops_det        - Flops for matrix determinant.
%   flops_digamma    - Flops for gammaln, digamma, and trigamma.
%   flops_div        - Flops for division.
%   flops_exp        - Flops for exponential.
%   flops_inv        - Flops for matrix inversion.
%   flops_mul        - Flops for real matrix multiplication.
%   flops_normpdfln  - Flops for normpdfln.
%   flops_pow        - Flops for raising to real power.
%   flops_randnorm   - Flops for randnorm.
%   flops_row_sum    - Flops for row sums.
%   flops_sample     - Flops for sample(p,n).
%   flops_solve      - Flops for matrix left division.
%   flops_solve_tri  - Flops for triangular left division.
%   flops_spadd      - Flops for sparse matrix addition.
%   flops_spmul      - Flops for sparse matrix multiplication.
%   flops_sqrt       - Flops for square root.
%
% Stand alone programs
%   matfile          - Read/write MAT files.
%   test_flops       - Compare time versus flops for various math operations.
%
% Graphics utilities
%  see graphics/Contents.m
%
% Demos
%   test_repmat,
%   test_solve_tri, ...


function quadrule_test01 ( )

%*****************************************************************************80
%
%% TEST01 tests CLENSHAW_CURTIS_RULE_COMPUTE and CLENSHAW_CURTIS_RULE_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  CLENSHAW_CURTIS_RULE_COMPUTE computes\n' );
  fprintf ( 1, '  a Clenshaw-Curtis quadrature rule.\n' );
  fprintf ( 1, '  CLENSHAW_CURTIS_RULE_SET sets\n' );
  fprintf ( 1, '  a Clenshaw-Curtis quadrature rule.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compare:\n' );
  fprintf ( 1, '    (X1,W1) from CLENSHAW_CURTIS_RULE_SET\n' );
  fprintf ( 1, '    (X2,W2) from CLENSHAW_CURTIS_RULE_COMPUTE\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '     Order        W1            W2            X1            X2\n' );
  fprintf ( 1, '\n' );

  for order = 1 : 3 : 10

    [ x1, w1 ] = clenshaw_curtis_rule_set ( order );
    [ x2, w2 ] = clenshaw_curtis_rule_compute ( order );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %8d\n', order );

    for i = 1 : order
      fprintf ( 1, '            %12f  %12f  %12f  %12f\n', ...
        w1(i), w2(i), x1(i), x2(i) );
    end

  end

  return
end


function [f] = spm_mc_fx_2(x,v,P)
% equations of motion for the mountain car problem using basis functions
% problem
% FORMAT [f] = spm_mc_fx_2(x,v,P)
%
% x   - hidden states
% v   - exogenous inputs
% P.p - parameters for gradient function:     G(x(1),P.p)
% P.q - parameters for cost or loss function: C(x(1),P.q)
%
% returns f = dx/dt = f  = [x(2);
%                           G - x(2)*C]*dt;
%
% where C determines divergence of flow x(2) at any position x(1).
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_mc_fx_2.m 3140 2009-05-21 18:38:17Z karl $

% gradient (G)
%--------------------------------------------------------------------------
G     = spm_DEM_basis(x.x(1),v,P.p);

% cost function (C)
%--------------------------------------------------------------------------
[C p] = spm_DEM_basis(x.x(1),v,x.c);

% flow
%--------------------------------------------------------------------------
dt    = 1/8;
f.x   = [x.x(2); G + x.x(2)*(C + P.c)]*dt;
f.c   = [P.q.*p(:) - x.c/4]*dt;

% true scalar potential gradient (see spm_moutaincar_fx)
%--------------------------------------------------------------------------
% if x(1) < 0
%     G  = 2*x(1) + 1;
% else
%     xx = x(1)^2;
%     G  = (1 + 5*xx)^(-1/2) - 5*xx/(1 + 5*xx)^(3/2) + (x(1)/2)^4;
% end


clear all, close all, clc
A = imread('../../CH01_SVD/DATA/dog.jpg');
B = rgb2gray(A);

%% Wavelet Compression
figure
[C,S] = wavedec2(B,4,'db1');

% Zero out all small coefficients and inverse transform
Csort = sort(abs(C(:)));
keepvec = [.1 .05 .01 .005 .002 .001];
for k=1:6
    figure
    keep = keepvec(k);
    % keep = 0.05;
    thresh = Csort(floor((1-keep)*length(Csort)));
    ind = abs(C)>thresh;
    Cfilt = C.*ind;

    % Plot Reconstruction
    Alow=uint8(waverec2(Cfilt,S,'db1'));
    imagesc(uint8(Alow))
    colormap gray

    colormap gray
    axis off
    axis tight
    set(gcf,'Position',[100 100 600 800])
    set(gcf,'PaperPositionMode','auto')
    %     print('-depsc2', '-loose', ['../figures/2DFFT_Compress',num2str(k)]);
        print('-dpng', '-loose', ['../figures/2DWavelet_Compress',num2str(k)]);

end


function a = rutis2_inverse ( )

%*****************************************************************************80
%
%% RUTIS2_INVERSE returns the inverse of the RUTIS2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Joan Westlake,
%    A Handbook of Numerical Matrix Inversion and Solution of
%    Linear Equations,
%    John Wiley, 1968,
%    ISBN13: 978-0471936756,
%    LC: QA263.W47.
%
%  Parameters:
%
%    Output, real A(4,4), the matrix.
%

%
%  Note that the matrix entries are listed by row.
%
  a(1:4,1:4) = [ ...
     28.0,  -22.0, -1.0, -1.0; ...
    -22.0,   28.0, -1.0, -1.0; ...
     -1.0,   -1.0, 17.0, -8.0; ...
     -1.0,   -1.0, -8.0, 17.0 ];

  a(1:4,1:4) = a(1:4,1:4) / 50.0;

  return
end


%HOMTRANS Apply a homogeneous transformation
%
% P2 = HOMTRANS(T, P) applies the homogeneous transformation T to the points
% stored columnwise in P.
%
% - If T is in SE(2) (3x3) and
%   - P is 2xN (2D points) they are considered Euclidean (R^2)
%   - P is 3xN (2D points) they are considered projective (P^2)
% - If T is in SE(3) (4x4) and
%   - P is 3xN (3D points) they are considered Euclidean (R^3)
%   - P is 4xN (3D points) they are considered projective (P^3)
%
% P2 and P have the same number of rows, ie. if Euclidean points are given
% then Euclidean points are returned, if projective points are given then
% projective points are returned.
%
% TP = HOMTRANS(T, T1) applies homogeneous transformation T to the
% homogeneous transformation T1, that is TP=T*T1.  If T1 is a 3-dimensional
% transformation then T is applied to each plane as defined by the first two
% dimensions, ie. if T is NxN and T1 is NxNxM then the result is NxNxM.
%
% Notes::
% - If T is a homogeneous transformation defining the pose of {B} with respect to {A},
%   then the points are defined with respect to frame {B} and are transformed to be
%   with respect to frame {A}.
%
% See also E2H, H2E, RTBPose.mtimes.

% 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 pt = homtrans(T, p)

    if numrows(p) == numrows(T)
        if ndims(p) == 3
            % homtrans(T1, T2)
            pt = [];
            for i=1:size(p,3)
                pt = cat(3, pt, T*p(:,:,i));
            end
        else
            % points are in projective coordinates
            pt = T * p;
        end
    elseif (numrows(T)-numrows(p)) == 1
        % points are in Euclidean coordinates, promote to homogeneous
        pt = h2e( T * e2h(p) );
    else
        error('SMTB:homtrans:badarg', 'matrices and point data do not conform')
    end


function p4 = angle_half_2d ( p1, p2, p3 )

%*****************************************************************************80
%
%% ANGLE_HALF_2D finds half an angle in 2D.
%
%  Discussion:
%
%    The original angle is defined by the sequence of points P1, P2 and P3.
%
%    The point P4 is calculated so that:
%
%      (P1,P2,P4) = (P1,P2,P3) / 2
%
%        P1
%        /
%       /   P4
%      /  .
%     / .
%    P2--------->P3
%
%    Thanks to Cesar Fraga Bobis for pointing out a typographical error in
%    a previous version of this routine.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(2,1), P2(2,1), P3(2,1), points defining the angle.
%
%    Input, real P4(2,1), a point defining the half angle.
%    The vector P4 - P2 will have unit norm.
%
  p4(1:2,1) = 0.5 * ( ...
      ( p1(1:2,1) - p2(1:2,1) ) / sqrt ( sum ( ( p1(1:2,1) - p2(1:2,1) ).^2 ) ) ...
    + ( p3(1:2,1) - p2(1:2,1) ) / sqrt ( sum ( ( p3(1:2,1) - p2(1:2,1) ).^2 ) ) );

  p4(1:2,1) = p2(1:2,1) + p4(1:2,1) / sqrt ( sum ( ( p4(1:2,1) ).^2 ) );

  return
end


function f = dtlz2a(x)

alpha = 1;

%% g function
sum = 0;
for i = 3:8
sum = sum + (x(:,i)-0.5)^2;
end
g = sum;

%% cost functions
f(:,1) = (1+g)*cos(((x(:,1)^alpha)*pi)/2)*cos(((x(:,2)^alpha)*pi)/2);
f(:,2) = (1+g)*cos(((x(:,1)^alpha)*pi)/2)*sin(((x(:,2)^alpha)*pi)/2);
f(:,3) = (1+g)*sin(((x(:,1)^alpha)*pi)/2);
end


%% 2D Advection-Diffusion D2Q4
% by Manuel Diaz
clc; clear; %close all;

%% Domain Grid
L = 100;    % Length
H = 100;    % Height
n = 100;    % Nodes in x
m = 100;    % Nodes in y
dx = L/n; dy = H/m; dt = 1;
x = 1:dx:L; y = 1:dy:H;

%% Initial Condition
f1   = zeros(m,n);
f2   = zeros(m,n);
f3   = zeros(m,n);
f4   = zeros(m,n);
feq1 = zeros(m,n);
feq2 = zeros(m,n);
feq3 = zeros(m,n);
feq4 = zeros(m,n);
rho  = zeros(m,n); % initial value of the dependent variable rho = T(x,t)

%% Constants
u     = 0.1;
v     = -0.2; % <- interesting result if we change to -v!
ck    = dx/dt;
csq   = (dx^2)/(dt^2);
alpha = 0.25;
omega = 1/(2*alpha/(dt*csq)+0.5);
w     = [1/4,1/4,1/4,1/4];
cx    = [  1, -1,  0,  0];
cy    = [  0,  0,  1, -1];
link  = [  1,  2,  3,  4];
tEnd  = 400; % time steps

%% Movie Parameters
tPlot = 10; frames= tEnd/tPlot; count = 0;
figure(1)
colordef white %black

%% Boundary Conditions
twall = 1;

%% Main Loop
for cycle = 2:tEnd;
    % Collision process
    rho = f1 + f2 + f3 + f4;

    feq1 = 0.25 * rho *(1+2*u/ck);
    feq2 = 0.25 * rho *(1-2*u/ck);
    feq3 = 0.25 * rho *(1+2*v/ck);
    feq4 = 0.25 * rho *(1-2*v/ck);

    f1 = w(1) * rho;
    f2 = w(2) * rho;
    f3 = w(3) * rho;
    f4 = w(4) * rho;

    f1 = omega * feq1 + (1-omega) * f1;
    f2 = omega * feq2 + (1-omega) * f2;
    f3 = omega * feq3 + (1-omega) * f3;
    f4 = omega * feq4 + (1-omega) * f4;

    % Streaming process
    f1 = stream2d( f1 , [cy(1),cx(1)]);
    f2 = stream2d( f2 , [cy(2),cx(2)]);
    f3 = stream2d( f3 , [cy(3),cx(3)]);
    f4 = stream2d( f4 , [cy(4),cx(4)]);

    % Boundary conditions
    f1(:,1) = twall*w(1) + twall*w(2) - f2(:,1); %Dirichlet BC
    f3(:,1) = twall*w(3) + twall*w(4) - f4(:,1); %Dirichlet BC

    f1(m,:) = 0;        %Dirichlet BC
    f2(m,:) = 0;        %Dirichlet BC
    f3(m,:) = 0;        %Dirichlet BC
    f4(m,:) = 0;        %Dirichlet BC

    f1(:,n) = 0;        %Dirichlet BC
    f2(:,n) = 0;        %Dirichlet BC
    f3(:,n) = 0;        %Dirichlet BC
    f4(:,n) = 0;        %Dirichlet BC

    f1(1,:) = f1(2,:);  %Neumann BC
    f2(1,:) = f2(2,:);  %Neumann BC
    f3(1,:) = f3(2,:);  %Neumann BC
    f4(1,:) = f4(2,:);  %Neumann BC

    % Visualization
    if mod(cycle,tPlot) == 1
        count = count + 1;
        contourf(rho)
        colormap hot
        colorbar('location','southoutside')
        axis tight
        M(count) = getframe;
    end

    % Animated gif file
    if mod(cycle,tPlot) == 1
        F = getframe;
        if count == 1
            [im,map] = rgb2ind(F.cdata,256,'nodither');
            im(1,1,1,tEnd/tPlot) = 0;
        end
        im(:,:,1,count) = rgb2ind(F.cdata,map,'nodither');
    end
end

%% Make Movie
movie(M,1,10); % movie(M,n,fps)

%% Export to Gif
imwrite(im,map,'advec_diff_d2q4.gif','DelayTime',0,'LoopCount',3)


%rate = mimo_iso_conv(nn,P,error,tx,rx,K) computes the converse bound on
%the maximal channel coding rate over a quasi-static MIMO Rician fading
%channel with isotropic inputs.
%
%The function takes the following inputs:
%
%nn: blocklength, scalar/vector;
%P: total input power (linear scale), scalar;
%error: block error probability, scalar;
%tx: number of transmit antennas, scalar;
%rx: number of receive antennas, scalar;
%K:  rician K factor, scalar; the default value is 0.

function rate_c = mimo_iso_conv(nn,P,error,tx,rx,K)
if (nargin < 6) || isempty(K)
	K = 0;
end
loop_h=round(1000/error);

P=P/tx;

rate_c=[];

min_tx_rx = min(tx,rx);

eig_vec=zeros( min_tx_rx, loop_h);

for ij = 1:loop_h
    h = (randn(tx,rx)+1i*randn(tx,rx))*sqrt(0.5/(K+1)) + sqrt(K/(K+1));
    %eigenvalues of H^{H}*Q*H, where Q is a scaled identity matrix P*I
    eig_vec(:,ij)= P* abs(svd( h)).^2;
end

%nn=[60:10:200 220:20:500 550:50:1000];

for n=nn
    %information density under P
    S_n = sum( n*log(1 + eig_vec)) + n*min_tx_rx  - sum(ncx2rnd(2*n, 2*n ./ eig_vec).* eig_vec./ (1 + eig_vec))/2;

    S_n_sort=sort(S_n);

    tau = (error + 1/loop_h ): 1/loop_h : 2*error;

    gamma_n_array = S_n_sort(round(tau*loop_h));

    %lower-bounding beta_{error} using the standard inequality
    y_a = gamma_n_array/n - log(tau-error)/n;
    %optimizing over gamma
    y_min=min(y_a);

    rate =y_min./log(2);
    rate_c=[rate_c,rate];
end

%


function s = rectangular_plate_analytical_solution (a, b, h, E, nu, q0)
%
% INPUT
%   a:  length of the rectangular section
%   b:  height of the rectangular section
%   h:  thickness of the plate
%   E:  Young modulus
%   nu: Poisson ratio
%   q0: distributed load
%
% OUTPUT
%   displ: maximum displacement

x=a/2;
y=b/2;
%pi = %pi;
D=E*h^3/(12*(1-nu^2));
ks = 16*q0*a^4*b^4/(pi^6*D);
km = 16*q0*a^4*b^4/(pi^4);

s=0;
mx=0;
my=0;
n = 1;
nMax = 1001;
while n<=nMax
    m = 1;
    while m<=nMax
        den = (n^2*a^2 + m^2*b^2)^2;
        sinA = sin(m*pi*x/a);
        sinB = sin(n*pi*y/b);
        knm = sinA*sinB/(m*n*den);
        s = s + ks*knm;
        mx = mx + km*knm*((m/a)^2 + nu*(n/b)^2);
        my = my + km*knm*(nu*(m/a)^2 + (n/b)^2);
        m = m+2;
    end;
    n = n+2;
end

x=0;
y=0;
kmxy = -(1-nu)*16*q0*a^3*b^3/(pi^4);
mxy = 0;
n=1;
while n<=nMax
    m = 1;
    while m<=nMax
        den = (n^2*a^2 + m^2*b^2)^2;
        cosA = cos(m*pi*x/a);
        cosB = cos(n*pi*y/b);
        mxy = mxy + kmxy*cosA*cosB/den;
        m = m+2;
    end;
    n = n+2;
end


%Program for SVD-based Image Quality Measure

%Program Description
% This program outputs a graphical & numerical image quality measure
% based on Singular Value Decomposition.
% For details on the implementation, please refer
% Aleksandr Shnayderman, Alexander Gusev, and Ahmet M. Eskicioglu,
% "An SVD-Based Grayscale Image Quality Measure for Local and Global Assessment",
% IEEE TRANSACTIONS ON IMAGE PROCESSING, VOL. 15, NO. 2, FEBRUARY 2006.
%
%Author : Athi Narayanan S
%Student, M.E, EST,
%K.S.R College of Engineering
%Erode, Tamil Nadu, India.
%s_athi1983@yahoo.co.in
%http://sites.google.com/site/athisnarayanan/

%Sample Usage

clc;
close all;
clear all;

%Read Input Reference Gray Image
refImg=imread('lena256.bmp');
figure;subplot(121);
imshow(refImg);title('Orginal Image');

%Read Input Distorted Gray Image
distImg = imread('gaussian_noise_10.bmp');
subplot(122);
imshow(distImg);title('Distorted Image');

blkSize = 8;

[graMeasure, scaMeasure] = SVDQualityMeasure(refImg, distImg, blkSize);

figure;imshow(graMeasure);title('Graphical Measure');
disp('Numerical Measure');
disp(scaMeasure);


pb=1e-6;
data=[];
while pb<0.5
    data=[data;[pb,geterror(pb)]];
    pb=pb*1.5;
end
plot(log(data(:,1))/log(10),log(data(:,2))/log(10));


function s=p2s(p)
% function s=p2s(p)
%
% DESCRIPTION
%   Converts from a multipoly polynomial to a symbolic toolbox polynomial.
%
% INPUTS
%   p: Polynomial created using the multipoly toolbox
%
% OUTPUTS
%   s: Polynomial created using the symbolic toolbox
%
% SYNTAX
%   s = p2s(p)
%
% See also s2p

% 1/30/2003  PJS  Initial Coding
% 11/23/2010 PJS  Updated for matrix polynomials

% Get polynomial info
[nr nc] = size(p);
[nt,nv] = size(p.degmat);
coef = p.coefficient;
deg = p.degmat;
var = p.varname;

% Create symbolic toolbox variables
for i1 = 1:nv
    eval(['syms ' var{i1} ' real']);
end

% Construct monomial by monomial
s = zeros(nr,nc);
for i1 = 1:nt
    termexp = deg(i1,:);
    idx = find(termexp);
    term = 1;
    for i2 = 1:length(idx)
        eval(['term = term*(' var{idx(i2)} ')^termexp(idx(i2));']);
    end
    s = s + reshape(coef(i1,:),[nr,nc])*term;
end


function value = p24_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P24_F evaluates the integrand for problem 24.
%
%  Dimension:
%
%    DIM_NUM is arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integral Parameters:
%
%    C(1:DIM_NUM) defaults to 0.0.
%
%    A typical, more difficult problem, has
%      C(I) = I**(1/3)
%
%    Call P24_R8VEC to get or set C.
%
%  Integrand:
%
%    F(X) = product (   ( abs ( 4 * X(1:DIM_NUM) - 2 ) + C(1:DIM_NUM) )
%                     / ( 1 + C(1:DIM_NUM) )
%                   )
%
%  Exact Integral:
%
%    1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Stephen Joe, Frances Kuo,
%    Remark on Algorithm 659:
%    Implementing Sobol's Quasirandom Seqence Generator,
%    ACM Transactions on Mathematical Software,
%    Volume 29, Number 1, March 2003, pages 49-57.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  c = [];
  c = p24_r8vec ( 'G', 'C', dim_num, c );

  value(1:point_num) = 0.0;

  for point = 1 : point_num

    value(point) = prod ...
    ( ...
        ( abs ( 4.0 * x(1:dim_num,point) - 2.0 ) + c(1:dim_num)' ) ...
      ./ ( 1.0 + c(1:dim_num)' ) ...
    );
  end

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

  return
end


% Find all paths from a start to an end node,
% bounded by a constant, using depth-first-search.
%
% Source: Idea from 6.002x, Spring 2014.
% Note: Uses recursion.
%
% INPUTs: graph structure (a nxn adjacency matrix)
%         s - start node
%         e - end node
%         upperBound = 5, some constant bounding the
%                    length of the path; typically size(adj,1)-1
%         allPaths = {}, path = []; are by default empty,
%                                  serving the recursion
%
% OUTPUTs: a list {} of all shortest paths from "s" to "e",
%                                shorter than "upperBound"
%
% Other routines used: kneighbors.m, DFS.m
% GB: last updated Oct 27 2014

function allPaths = DFS(graph, s, e, allPaths = {}, path = [], upperBound = 5)

        path = [path s];

        if s == e
            if length(path)-1<=upperBound   % this can be modified to include edge weights [(1,2,{}),..]
                allPaths{length(allPaths)+1} = path;
            end
        else
            if length(path)-1<=upperBound

                kneigh = kneighbors(graph, s, 1);

                for node=1:length(kneigh)

                    node = kneigh(node);

                    if length(find(path==node))==0   % avoid cycles
                        allPaths = DFS(graph, node, e, allPaths, path, upperBound);
                    end

                end
            end
        end


function geometry_test085 ()

%*****************************************************************************80
%
%% TEST085 tests PLANE_EXP_PRO3.
%
%  Discussion:
%
%    Projection is ( -1, 1, 1 ).
%    Projection is ( 4, 5, -8 ).
%    Projection is ( 0.33, 0.33, 0.33).
%    Projection is ( 5.33, -1.66, -2.66 ).
%    Projection is ( -1, 1, 1 ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2009
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;
  ntest = 5;

  p1 = [ 1.0, 0.0, 0.0 ];
  p2 = [ 0.0, 1.0, 0.0 ];
  p3 = [ 0.0, 0.0, 1.0 ];
  po = [ ...
    0.0,  2.0,  2.0;  ...
    4.0,  5.0, -8.0;  ...
    0.25, 0.25, 0.25; ...
    5.0, -2.0, -3.0;  ...
   -2.0,  0.0,  0.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST085\n' );
  fprintf ( 1, '  PLANE_EXP_PRO3 projects an object point\n' );
  fprintf ( 1, '    orthographically into a plane.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '            PO                PP\n' );
  fprintf ( 1, '\n' );

  pp = plane_exp_pro3 ( p1, p2, p3, ntest, po );

  for i = 1 : ntest
    fprintf ( 1, '  %10f  %10f  %10f  %10f  %10f  %10f\n', ...
      po(1:dim_num,i), pp(1:dim_num,i) );
  end

  return
end


%% RATE OF CONVERGENCE OF PRIMAL-DUAL WEAK-GALERKIN for DIV-CURL SYSTEM
%
% This example is to show the rate of convergence on an L-shaped domain with a singular solution.
%
% See also DivCurl3PDWG
%
% Reference: A New Numerical Method for Div-Curl Systems with Low Regularity Assumptions
% S. Cao, C. Wang, J. Wang, https://arxiv.org/abs/2101.03466
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

clear; close all;
%%
maxIt = 4;
N = zeros(maxIt,1);
h = zeros(maxIt,1);
errL2UTotal = zeros(maxIt,1);
errqlambdaTotal = zeros(maxIt,1);
errSTotal = zeros(maxIt,1);
errL2QuTotal = zeros(maxIt,1); % \|Q_h u - u \|

%% Generate an initial mesh
[node,elem] = cubemesh([-1,1,-1,1,-1,1],1/2);
% [node,elem] = delmesh(node,elem,'(x < 0 & y > 0) | (z<0.5)');

% [node,elem] = delmesh(node,elem,'(x < 0 & y < 0) | (z<0) | (z>0.5)'); %
% for gradient

[node,elem] = delmesh(node,elem,'(x > 0 & y < 0) | (z<0) | (z>0.5)');
% for curl
bdFlag = setboundary3(node,elem,'Dirichlet');

%% data
pde = DataDivCurlSingular4;
option.inverseh = true;

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    [soln,eqn,info] = DivCurl3PDWG(node,elem,bdFlag,pde,option);
    N(k) = size(elem,1);
%     h(k) = (36./N(k)).^(1/3);
    h(k) = 1/2^k;

    errL2UTotal(k) = eqn.errorU;
    errqlambdaTotal(k) = eqn.errorqlambda;
    errSTotal(k) = eqn.errorS;

    [errL2QuTotal(k), ~] = getL2error3vec(node,elem,soln.u,pde);
%     [errL2QuTotal(k), ~] = getL2error3(node,elem,pde.exactu, soln.u);

    fprintf('PDWG method with h:     %3g \n', h(k));
    fprintf('Number of Dof:          %d \n', sum(eqn.freeDof));
    fprintf('L2 error in u:          %6g \n', errL2UTotal(k));
    fprintf('L2 error in Qu-u_h:     %6g \n', errL2QuTotal(k));
    fprintf('L2 error in (q,lambda): %6g \n', errqlambdaTotal(k));
    fprintf('L2 error in s:          %6g \n\n\n', errSTotal(k));

    % refine mesh
   if k < maxIt; [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag); end
end

%%
visualizationPDwgDivCurl;


function pass = test_nonlinSysDamping_C2(pref)
% Test 2x2 system (sin/cos) where damping is required
%
% Asgeir Birkisson, May 2014.

if ( nargin == 0 )
    pref = cheboppref;
end

tol = 1e-10;

% Smooth domain:
d = [-pi pi];
A = chebop(@(x,u,v) [u - diff(v,2) + (1-x.^2).*u.^2; diff(u,2) + sin(v)],d);
A.lbc = @(u,v) [u-1; v+1];
A.rbc = @(u,v) [v-1/2; diff(v)];
x = chebfun('x',d);
f = [ 0*x ; 0*x ];

%% CHEBCOLLOC2
pref.discretization = @chebcolloc2;
[u1, u2, info] = solvebvp(A, f, pref);

% Want to check BCs as well.
bcFunLeft = chebfun(A.lbc(u1,u2));
bcFunRight = chebfun(A.rbc(u1,u2));

pass(1) = info.error < tol;
pass(2) = norm(bcFunLeft(d(1))) < tol && norm(bcFunRight(d(end))) < tol;

end


function [ x, seed ] = pearson_05_sample ( a, b, c, seed )

%*****************************************************************************80
%
%% PEARSON_05_SAMPLE samples the Pearson 5 PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < A, 0.0 < B.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  a2 = 0.0;
  b2 = b;
  c2 = 1.0 / a;

  [ x2, seed ] = gamma_sample ( a2, b2, c2, seed );

  x = c + 1.0 / x2;

  return
end


function varargout = slogfrac(varargin)
%logfrac
%
% y = SLOGFRAC(x)
%
% Computes/declares log(1 + x(1)/x(2))

switch class(varargin{1})
    case 'double'
        x = varargin{1};
        % Safe version with defined negative values (helps fmincon when
        % outside feasible region)
        if all(x==0)
            varargout{1} = log(2);% ?definition...
        elseif x(1)==0
            varargout{1} = log(1);
        else
            aux = 1 + (x(1)./(x(2)+sqrt(eps)));
            if aux <= 0
                varargout{1} = -15;
            else
                varargout{1} = log(1 + (x(1)./(x(2)+sqrt(eps))));
            end

        end

    case 'sdpvar'
        if min(size(varargin{1}))>1
            error('SLOGFRAC only defined for vector arguments');
        else
            varargout{1} = yalmip('define',mfilename,varargin{1});
        end

    case 'char'
        X = varargin{3};
        Y = X(2);
        X = X(1);
        F = [];
        operator = struct('convexity','none','monotonicity','none','definiteness','none','model','callback');
        operator.range = [-inf inf];
        operator.domain = [-inf inf];
        operator.bounds = @bounds;
        operator.derivative = @(x) ([1./(x(1)+x(2)+sqrt(eps));1./(x(1)+x(2)+sqrt(eps))-(x(2)+sqrt(eps)).^-1]);
        varargout{1} = F;
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error('SDPVAR/SLOGFRAC called with CHAR argument?');
end

function [L,U] = bounds(xL,xU)

% Derive bounds on x1/x2
z = [xL(1)./xL(2) xU(1)./xL(2) xL(1)./xU(2) xU(1)./xU(2)];
fL = min(z);
fU = max(z);

if fL <= -1
    L = -inf;
else
    L = log(1 + fL);
end
if fU <= -1
    U = -inf;
else
    U = log(1 + fU);
end


function [ y, m, d, f ] = jed_to_ymdf_julian3 ( jed )

%*****************************************************************************80
%
%% JED_TO_YMDF_JULIAN3 converts a JED to a Julian YMDF date.
%
%  Discussion:
%
%    The theory behind this routine is very clean.  The Julian
%    calendar has cycles of 1 and 4 years, and we can analyze a date
%    by determining where it lies within these cycles.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Reingold, Nachum Dershowitz, Stewart Clamen,
%    Calendrical Calculations, II: Three Historical Calendars,
%    Software - Practice and Experience,
%    Volume 23, Number 4, pages 383-404, April 1993.
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer Y, M, D, real F,
%    the YMDF date.
%
  cycle_length = 1461;
  year_length = 365;

  jed_epoch = epoch_to_jed_julian ( );

  j = floor ( jed - jed_epoch );
  f1 = ( jed - jed_epoch ) - j;

  if ( f1 < 0.0 )
    f1 = f1 + 1.0;
    j = j - 1;
  end

  d0 = j;
  n4 = 0;

  while ( d0 <= 0 )
    d0 = d0 + cycle_length;
    n4 = n4 - 1;
  end

  n4 = n4 + floor ( d0 / cycle_length );
  d1 = i4_modp ( d0, cycle_length );
  n1 = floor ( d1 / year_length );
  d2 = i4_modp ( d1, year_length );

  if ( n1 == 4 )
    j1 = 366;
    y1 = 4 * n4 + n1;
  else
    j1 = d2 + 1;
    y1 = 4 * n4 + n1 + 1;
  end
%
%  Any year before 1 AD must be moved one year further back, since
%  this calendar does not include a year 0.
%
  y1 = y_astronomical_to_common ( y1 );

  [ y, m, d, f ] = yjf_to_ymdf_julian ( y1, j1, f1 );

  return
end


function [ud,UD_up,UD_dist] = distort(up,dist)

% DISTORT  Distort projected point with radial distortion.
%   DISTORT(UP,DIST) computes the position in the image plane of the
%   distorted pixel corresponding to the projected pixel UP. DIST is the
%   vector of distortion parameters. Distortion is calculated with the
%   radial distortion model:
%
%     UD = UP * (1 + k2*r^2 + k4*r^4 + k6*r^6 + ...)
%
%   where r^2  = UP(1)^2 + UP(2)^2
%     and DIST = [k4 k4 k6 ...]
%
%   [ud,UD_up,UD_dist] = DISTORT(...) returns Jacobians wrt UP and DIST.
%
%   See also PINHOLE.
%

%   FIXME: RMAX works not so nice
%   DISTORT(...,RMAX) accepts the maximum image radius to limit the effect
%   of distortion to the image plane. The image radius is one half of the
%   image diagonal. In case norm(UP) > RMAX then UD = RMAX/norm(UP)*UP. The
%   Jacobians are trivially computed just for argument consistency: UDup =
%   eye(2) and UDdist = zeros(2,length(dist)).

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

r2 = sum(up.^2);

n = length(dist);

if n == 0  % No distortion
    ud = up;

    if size(up,2) == 1
        UD_up   = eye(2);
        UD_dist = zeros(2,0);
    else
        error('??? Jacobians not available for multiple points.')
    end

else       % Distortion

    if size(up,2) == 1   % One only pixel

        nn    = 1:n;           % orders vector
        r2n   = r2.^nn;        % powers vector
        ratio = 1 + r2n*dist;  % distortion ratio

        ud    = ratio*up;

        if nargout > 1 % jacobians

            dnn    = 2:2:2*n;
            dr2n   = r2.^(0:n-1);
            dratio = (dnn.*dr2n)*dist;

            UD_up = ratio*eye(2) + dratio*(up*up');

            UD_dist = kron(up,r2n);
        end

    else  % vectorized operation for multiple pixels.

        ratio = ones(size(r2));
        for i=1:n
            ratio = ratio + dist(i)*r2.^i;
        end

        ud = up.*[ratio;ratio];

        if nargout > 1 % jacobians

            error('??? Jacobians not available for multiple points.')

        end
    end
end

return

%% Build jacobians up to order 3
% For higher orders:
%   1/ add k8,k10,k(2n)... to syms line below for an order n.
%   2/ add k8,k10,k(2n)... to vector dist below for an order n.
%   3/ Execute this cell (Apple+return keys)
%   4/ Copy the results in UDup and UDdist above, caring for the matrices'
%   opening and closing brackets (do not forget them).

syms u1 u2 k2 k4 k6 k8 real
up   = [u1;u2];
dist = [k2;k4;k6;k8];
ud   = distort(up,dist);

UDup   = simple(jacobian(ud,up))
UDdist = simple(jacobian(ud,dist))

%% test

[ud, UD_up, UD_dist] = distort(up,dist);

simplify(UD_up - jacobian(ud,up))
simplify(UD_dist - jacobian(ud,dist))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function invT = TransInv(T)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a transformation matrix T.
% Returns its inverse.
% Uses the structure of transformation matrices to avoid taking a matrix
% inverse, for efficiency.
% Example Input:
%
% clear; clc;
% T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
% invT = TransInv(T)
%
% Ouput:
% invT =
%     1     0     0     0
%     0     0     1    -3
%     0    -1     0     0
%     0     0     0     1

[R, p] = TransToRp(T);
invT = [R', -R' * p; 0, 0, 0, 1];
end


function Pb=parbound(P,Ps)

% function Pb=parbound(P,Ps)
% ------------------------------------------------------------------------
% Maps the values in the vector P to Pb according to upper and lower limits
% set in the input structure Ps.
%
% Ps.ub -> Upper bound
% Ps.lb -> Lower bound
% Ps.c -> Centre of the output interval (i.e. initial parameter)
% Ps.f -> Scale factor such that at for the input range
% [-f*(ub-lb),f*(ub-lb)] the width of the output range is Ps.t*(ub-lb)
% Ps.t -> Factor (0-1) of the output range width in relation to the intput
% range width
%
%
% %% EXAMPLE
% Ps.f=2;
% Ps.t=0.9;
% Ps.ub=10;
% Ps.lb=0;
% Ps.c=7;
%
% w=Ps.ub-Ps.lb;
% P=linspace(Ps.c-Ps.f*w,Ps.c+Ps.f*w,500);
% Pb=parbound(P,Ps);
% hf1=cFigure; hold on; grid on;
% xlabel('P','FontSize'); ylabel('Pb','FontSize');
% hf=plot(P,Pb,'b-','LineWidth',5);
% hf=plot(P,(Ps.ub-(0.5*(1-Ps.t)*w))*ones(size(P)),'g-','LineWidth',5);
% hf=plot(P,(Ps.lb+(0.5*(1-Ps.t)*w))*ones(size(P)),'g-','LineWidth',5);
% hf=plot(P,Ps.ub*ones(size(P)),'r-','LineWidth',5);
% hf=plot(P,Ps.lb*ones(size(P)),'r-','LineWidth',5);
% axis equal; axis tight; ylim([Ps.lb Ps.ub]);
%
%
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 06/02/2012
%------------------------------------------------------------------------

w=abs(Ps.ub-Ps.lb); %Interval width
b=Ps.f*w; %Point at which the output interval width equals t*w
a=tan(Ps.t*0.5*pi);
x=(P-Ps.c)./(b./a);
Pb=(w.*(0.5+(atan(x)./pi)))+Ps.lb;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 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/>.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%          Inverse Short-Time Fourier Transform        %
%               with MATLAB Implementation             %
%                                                      %
% Author: M.Sc. Eng. Hristo Zhivomirov       12/26/13  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x, t] = istft(stft, h, nfft, fs)

% function: [x, t] = istft(stft, h, nfft, fs)
% stft - STFT matrix (only unique points, time across columns, freq across rows)
% h - hop size
% nfft - number of FFT points
% fs - sampling frequency, Hz
% x - signal in the time domain
% t - time vector, s

% estimate the length of the signal
coln = size(stft, 2);
xlen = nfft + (coln-1)*h;
x = zeros(1, xlen);

% form a periodic hamming window
win = hamming(nfft, 'periodic');

% perform IFFT and weighted-OLA
if rem(nfft, 2)                     % odd nfft excludes Nyquist point
    for b = 0:h:(h*(coln-1))
        % extract FFT points
        X = stft(:, 1 + b/h);
        X = [X; conj(X(end:-1:2))];

        % IFFT
        xprim = real(ifft(X));

        % weighted-OLA
        x((b+1):(b+nfft)) = x((b+1):(b+nfft)) + (xprim.*win)';
    end
else                                % even nfft includes Nyquist point
    for b = 0:h:(h*(coln-1))
        % extract FFT points
        X = stft(:, 1+b/h);
        X = [X; conj(X(end-1:-1:2))];

        % IFFT
        xprim = real(ifft(X));

        % weighted-OLA
        x((b+1):(b+nfft)) = x((b+1):(b+nfft)) + (xprim.*win)';
    end
end

W0 = sum(win.^2);                   % find W0
x = x.*h/W0;                        % scale the weighted-OLA

% calculate the time vector
actxlen = length(x);                % find actual length of the signal
t = (0:actxlen-1)/fs;               % generate time vector

end


function [ w_sig, m_sig ] = mixer( L, bits, lower, upper, K )
%MIXER is the mixer signal to smooth data and spread it easier.
%
%   INPUTS VARIABLES
%       L     : Length of segment
%       bits  : Binary sequence (1xm char)
%       K     : Length to be smoothed
%       upper : Upper bound of mixer signal
%       lower : Lower bound of mixer signal
%
%   OUTPUTS VARIABLES
%       m_sig : Mixer signal to spread data
%       w_sig : Smoothed mixer signal
%
%   Kadir Tekeli (kadir.tekeli@outlook.com)

if (nargin < 4)
    lower = 0;
    upper = 1;
end

if (nargin < 5) || (2*K > L)
	K = floor(L/4) - mod(floor(L/4), 4);
else
    K = K - mod(K, 4);                       %Divisibility by 4
end

N = length(bits);                            %Number of segments
encbit = str2num(reshape(bits, N, 1))';      %char -> double
m_sig  = reshape(ones(L,1)*encbit, N*L, 1);  %Mixer signal
c      = conv(m_sig, hanning(K));            %Hann windowing
wnorm  = c(K/2+1:end-K/2+1) / max(abs(c));   %Normalization
w_sig  = wnorm * (upper-lower)+lower;        %Adjusting bounds
m_sig  = m_sig * (upper-lower)+lower;        %Adjusting bounds

end

function out = hanning(L)
%HANNING() is a manual implentation of hanning window HANN() to be used
%   without Signal Processing Toolbox in MATLAB. Input must be a numeric
%   greater than zero, so that a hanning window will be generated using
%   input value as window length.
%
%   Kadir Tekeli (kadir.tekeli@outlook.com)

if isnumeric(L)
    L = round(L);
    if L == 1
        out = 1;
    elseif L>1 || L==0
        n   = (0:L-1)';
        out = .5*(1-cos((2*pi*n)/(L-1)));
    else
        error('Input must be greater than zero!');
    end
else
    error('Input must be numeric!');
end

end


% FIT_CUBIC_BEZIER Fit a cubic bezier spline (G1 continuous) to an ordered list
% of input points in any dimension, according to "An algorithm for automatically
% fitting digitized curves" [Schneider 1990].
%
%  Inputs:
%    d  #d by dim list of points along a curve to be fit with a cubic bezier
%      spline (should probably be roughly uniformly spaced). If d(0)==d(end),
%      then will treat as a closed curve.
%    error  maximum squared distance allowed
%  Output:
%    cubics #cubics list of 4 by dim lists of cubic control points
%


function R = euler2rotMat(phi, theta, psi)
    R(1,1,:) = cos(psi).*cos(theta);
    R(1,2,:) = -sin(psi).*cos(phi) + cos(psi).*sin(theta).*sin(phi);
    R(1,3,:) = sin(psi).*sin(phi) + cos(psi).*sin(theta).*cos(phi);

    R(2,1,:) = sin(psi).*cos(theta);
    R(2,2,:) = cos(psi).*cos(phi) + sin(psi).*sin(theta).*sin(phi);
    R(2,3,:) = -cos(psi).*sin(phi) + sin(psi).*sin(theta).*cos(phi);

    R(3,1,:) = -sin(theta);
    R(3,2,:) = cos(theta).*sin(phi);
    R(3,3,:) = cos(theta).*cos(phi);
end


function zi = lagrange_interp_nd_value ( m, n_1d, a, b, nd, zd, ni, xi )

%*****************************************************************************80
%
%% LAGRANGE_INTERP_ND_VALUE evaluates an ND Lagrange interpolant.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N_1D(M), the order of the 1D rule to be used
%    in each dimension.
%
%    Input, real A(M), B(M), the lower and upper interval endpoints in
%    each dimension.
%
%    Input, integer ND, the number of points in the product grid.
%
%    Input, real ZD(ND), the function evaluated at the points XD.
%
%    Input, integer NI, the number of points at which the interpolant
%    is to be evaluated.
%
%    Input, real XI(M,NI), the points at which the interpolant is to be
%    evaluated.
%
%    Output, real ZI(NI), the interpolant evaluated at the points XI.
%
  zi = zeros(ni,1);

  for j = 1 : ni

    w = ones ( nd, 1 );

    for i = 1 : m
      n = n_1d(i);
      x_1d(1:n) = cc_compute_points ( n );
      x_1d(1:n) = 0.5 * ( ( 1.0 - x_1d(1:n) ) * a(i) ...
                        + ( 1.0 + x_1d(1:n) ) * b(i) );
      value = lagrange_basis_1d ( n, x_1d, 1, xi(i,j) );
      w = r8vec_direct_product2 ( i, n, value, m, nd, w );
    end

    zi(j) = zd' * w;

  end

  return
end


function s = DotXBLAS(x,y)
%DOTXBLAS     Dot product 'as if' computed in 2-fold precision
%
%   res = DotXBLAS(x,y)
%
%On return, res approximates x'*y with accuracy as if computed
%  in 2-fold precision.
%
%Implements algorithm BLAS_ddot_x from
%  X. Li, J. Demmel, D. Bailey, G. Henry, Y. Hida, J. Iskandar,
%    W. Kahan, S. Kang, {S.}, A. Kapur, M. Martin, B. Thompson, {B.},
%    T. Tung, {T.}, D. Yoo: Design, Implementation and Testing of
%    Extended and Mixed Precision BLAS, ACM Trans. Math. Software,
%    2(28), p. 152-205, 2002.
%Requires 37n flops.
%
%Reference implementation! Slow due to interpretation!
%

% written  03/03/07     S.M. Rump
% modified 05/09/09     S.M. Rump  rounding to nearest, complex input
%

  if ( ~isreal(x) ) | ( ~isreal(y) )
    error('DotXBLAS for real input only')
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  s = 0;
  t = 0;
  for i=1:length(x)
    [h,r] = TwoProduct(x(i),y(i));
    [s1,s2] = TwoSum(s,h);
    [t1,t2] = TwoSum(t,r);
    s2 = s2 + t1;
    [t1,s2] = FastTwoSum(s1,s2);
    t2 = t2 + s2;
    [s,t] = FastTwoSum(t1,t2);
  end

  if rndold
    setround(rndold)
  end


function s = isstatv(pc)

%function s = isstatv(pc)
%
% Determines whether the set of partial correlations pc
% defines a stationary process
%
% It checks whether I-pc*pc'>0 for all pc's
%
% It does this by checking whether the singular values
% of all pc's are smaller than 1.
%

Lmax = size(pc,3)-1;
nons = 0;
for i = 1:Lmax,
   nons = nons+any(svd(pc(:,:,i+1))>1);
end
s = ~nons;


function [phi, lam, h, phiC] = cart2geod(X, Y, Z)

% SYNTAX:
%   [phi, lam, h, phiC] = cart2geod(X, Y, Z);
%   [phi, lam, h, phiC] = cart2geod(xyz);
%
% INPUT:
%   X = X axis cartesian coordinate
%   Y = Y axis cartesian coordinate
%   Z = Z axis cartesian coordinate
%
% OUTPUT:
%   phi = latitude
%   lam = longitude
%   h = ellipsoidal height
%   phiC = geocentric latitude
%
% DESCRIPTION:
%   Conversion from cartesian coordinates to geodetic coordinates.

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   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 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/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

%global a_GPS e_GPS

if nargin == 1
    Z = X(:, 3);
    Y = X(:, 2);
    X = X(:, 1);
end

a = GPS_SS.ELL_A;
e = GPS_SS.ELL_E;

%radius computation
r = sqrt(X.^2 + Y.^2 + Z.^2);

%longitude
lam = atan2(Y,X);

%geocentric latitude
phiC = atan(Z./sqrt(X.^2 + Y.^2));

%coordinate transformation
psi = atan(tan(phiC)/sqrt(1-e^2));

phi = atan((r.*sin(phiC) + e^2*a/sqrt(1-e^2) * (sin(psi)).^3) ./ ...
    			(r.*cos(phiC) - e^2*a * (cos(psi)).^3));

N = a ./ sqrt(1 - e^2 * sin(phi).^2);

%height
h = r .* cos(phiC)./cos(phi) - N;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  In this script, we perform phase transition analysis
%  of Orthogonal matching pursuit.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
clear all;
clc;
rng('default');
% Create the directory for storing results
[status_code,message,message_id] = mkdir('bin');
target_file_path = 'bin/mmv_phase_transition_snr_40db_s_32.mat';
N = 256;
S = 32;
pta = spx.pursuit.PhaseTransitionAnalysis(N);
pta.SNR = 40;
% pta.NumTrials = 100;
dict_model = @(M, N) spx.dict.simple.gaussian_dict(M, N);
data_model = @(N, K) spx.data.synthetic.SparseSignalGenerator(N, K, S).gaussian;
recovery_solver = @(Phi, K, y) spx.pursuit.joint.CoSaMP(Phi, K).solve(y).Z;
pta.run(dict_model, data_model, recovery_solver);
pta.save_results(target_file_path);


function S = oblateSurfaceArea(elli, varargin)
%OBLATESURFACEAREA  Approximated surface area of an oblate ellipsoid
%
%   S = oblateSurfaceArea(R1,R2)
%
%   Example
%   oblateSurfaceArea
%
%   See also
%   geom3d, ellipsoidSurfaceArea, prolateSurfaceArea
%

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

%% Parse input argument

if size(elli, 2) == 7
    R1 = elli(:, 4);
    R2 = elli(:, 5);

elseif size(elli, 2) == 1 && ~isempty(varargin)
    R1 = elli(:, 1);
    R2 = varargin{1};
end

assert(R1 < R2, 'First radius must be smaller than second radius');

% surface theorique d'un ellipsoide oblate
% cf http://fr.wikipedia.org/wiki/Ellipso%C3%AFde_de_r%C3%A9volution
e = sqrt(R2.^2 - R1.^2) ./ R2;
S = 2 * pi * R2.^2 + pi * R1.^2 * log((1 + e) ./ (1 - e)) ./ e;


function [T, Y]=FOChuaM(parameters, orders, TSim, Y0)
%
% Numerical Solution of the Fractional-Order Chua's System
% with piecewise linear nonlinearity defined by a memristor.
%
%   D^q1 x(t) = alpha(y(t) - x(t) + epsilon x(t) - W(w)x(t))
%   D^q2 y(t) = x(t) - y(t) + z(t)
%   D^q3 z(t) = -beta y(t) - gamma z(t)
%   D^q4 w(t) = x(t)
%   where  W(x) is defined as W_w(a, b, w);
%
% function [T, Y] = FOChuaM(parameters, orders, TSim, Y0)
%
% Input:    parameters - model parameters [alpha, beta, gamma, epsilon, a, b]
%           orders - derivatives orders [q1, q2, q3, q4]
%           TSim - simulation time (0 - TSim) in sec
%           Y0 - initial conditions [Y0(1), Y0(2), Y0(3), Y0(4)]
%
% Output:   T - simulation time (0 : Tstep : TSim)
%           Y - solution of the system (x=Y(1), y=Y(2), z=Y(3), w=Y(4))
%
% Author:  (c) Ivo Petras (ivo.petras@tuke.sk), 2010.
%

% time step:
h=0.005;
% number of calculated mesh points:
n=round(TSim/h);
%orders of derivatives, respectively:
q1=orders(1); q2=orders(2); q3=orders(3); q4=orders(4);
% constants of Chua's system:
alpha=parameters(1); beta=parameters(2); gamma=parameters(3);
epsilon=parameters(4); a=parameters(5); b=parameters(6);
% binomial coefficients calculation:
cp1=1; cp2=1; cp3=1; cp4=1;
for j=1:n
    c1(j)=(1-(1+q1)/j)*cp1;
    c2(j)=(1-(1+q2)/j)*cp2;
    c3(j)=(1-(1+q3)/j)*cp3;
    c4(j)=(1-(1+q4)/j)*cp4;
    cp1=c1(j); cp2=c2(j);
    cp3=c3(j); cp4=c4(j);
end
% initial conditions setting:
x(1)=Y0(1); y(1)=Y0(2); z(1)=Y0(3); w(1)=Y0(4);
% calculation of phase portraits /numerical solution/:
for i=2:n
    x(i)=(alpha*(y(i-1)-x(i-1)+epsilon*x(i-1)-W_w(a,b,w(i-1))*x(i-1)))*h^q1 - memo(x, c1, i);
    y(i)=(x(i)-y(i-1)+z(i-1))*h^q2 - memo(y, c2, i);
    z(i)=(-beta*y(i)-gamma*z(i-1))*h^q3 - memo(z, c3, i);
    w(i)=(x(i))*h^q4 - memo(w, c4, i);
end
for j=1:n
    Y(j,1)=x(j);
    Y(j,2)=y(j);
    Y(j,3)=z(j);
    Y(j,4)=w(j);
end
T=h:h:TSim;
%


function [param]=pp2chp(input)
% [param]=pp2chp(input)
% LQ controller for 2nd order processes.
% This function computes parameters of the controller (r0, r1, q0, q1, p0, p1).
% The dynamic behavoiour of the closed-loop is defined by LQ criterion
% Output of the controller is calculated follows:
%
%            r0 + r1*z^-1               q0 + q1*z^-1
% U(z^-1) = -------------- * W(z^-1) - -------------- * Y(z^-1)
%            p0 + p1*z^-1               p0 + p1*z^-1
%
% Transfer function of the controlled system is:
%
%               b1*z^-1 + b2*z^-2
% Gs(z^-1) = -----------------------
%             1 + a1*z^-1 + a2*z^-2
%
% Input:
%   input(1:4) ... [a1 b1 a2 b2]
%   input(5) ... type of reference signal (1-step, 2-ramp, 3-sin)
%   input(6) ... frequency [Hz] (used if reference signal is sin wave)
%   input(7) ... sample time (used if reference signal is sin wave)
%   input(8) ... penalisation of u (constant fi in LQ criterion)
% Output: param ... controller parameters  [r0; r1; q0; q1; p0; p1];

a1 = input(1);
b1 = input(2);
a2 = input(3);
b2 = input(4);
rs_type = input(5);
rs_freq = input(6);
T0 = input(7);
fi = input(8);

% LQ: AP+BQ = D where D determined by spectral factorization
% A(z^-1)*fi*A(z) + B(Z^-1)*B(z) = D(z^-1)*delta*D(z)
%          M(z^-1)*M(z)          = D(z^-1)*delta*D(z)
m0 = fi*(1+a1^2+a2^2) + b1^2 + b2^2;
m1 = fi*a1*(1+a2) + b1*b2;
m2 = fi*a2;

la = m0/2 - m2 + sqrt( (m0/2+m2)^2 - m1^2 );
delta = (la + sqrt(la^2-4*m2^2)) / 2;
d2 = m2 / delta;
d1 = m1 / (delta+m2);
d0 = 1;
d3 = 0;

% FBFW controller: Y=BR/(AP+BQ)*W
% conditions: 1) AP+BQ=D
%             2) BR+FS=D  where W=H/F and S is any polynomial
% 1st condition:
%  A =  1 + a1*z^-1 + a2*z^-2   B =      b1*z^-1 + b2*z^-2
%  P = p0 + p1*z^-1             Q = q0 + q1*z^-1
% system of linear equations:
% [ 1  0   0  0]   [p0]   [d0]
% [a1  1  b1  0]   [p1]   [d1]
% [a2 a1  b2 b1] * [q0] = [d2]
% [ 0  a2  0 b2]   [q1]   [d3]
p0 = d0;
p1 = (d0*b2^2*a1-d0*b2*b1*a2-b2^2*d1+b1*b2*d2-b1^2*d3)/(-b2^2+a1*b1*b2-a2*b1^2);
q0 = -(d0*a1^2*b2-d0*a1*b1*a2-d0*a2*b2-d1*a1*b2+d1*b1*a2+b2*d2-b1*d3)/(-b2^2+a1*b1*b2-a2*b1^2);
q1 = -(d0*a2*a1*b2-d0*a2^2*b1-a2*b2*d1+b1*a2*d2+d3*b2-d3*b1*a1)/(-b2^2+a1*b1*b2-a2*b1^2);

% 2nd condition:
switch (rs_type)
case 1,     %step:
    % B = b1*z^-1 + b2*z^-2   F =  1 - z^-1
    % R = r0                  S = s0 + s1*z^-1 + s2*z^-2
    % [ 0    1    0   0]   [r0]   [d0]
    % [b1   -1    1   0]   [s0]   [d1]
    % [b2    0   -1   1] * [s1] = [d2]
    % [ 0    0    0  -1]   [s2]   [d3]
    r0 = (d0+d1+d2+d3)/(b1+b2);
    r1 = 0;
case 2,     %ramp
    % B =      b1*z^-1 + b2*z^-2   F =  1 -  2*z^-1 + z^-2
    % R = r0 + r1*z^-1             S = s0 + s1*z^-1
    % [ 0   0   1   0]   [r0]   [d0]
    % [b1   0  -2   1]   [r1]   [d1]
    % [b2  b1   1  -2] * [s0] = [d2]
    % [ 0  b2   0   1]   [s1]   [d3]
    r0 = (2*d0*b1+3*d0*b2+d1*b1+2*d1*b2+b2*d2-b1*d3)/(b1+b2)^2;
    r1 = -(d0*b1+2*d0*b2+d1*b2-b1*d2-2*b1*d3-d3*b2)/(b1+b2)^2;
case 3,     %sin(om*t/T0)
    om = 2*pi*rs_freq*T0;
    % B =      b1*z^-1 + b2*z^-2   F = 1 -  2*cos(om)*z^-1 + z^-2
    % R = r0 + r1*z^-1             S = s0 + s1*z^-1
    % [ 0   0           1           0]   [r0]   [d0]
    % [b1   0  -2*cos(om)           1]   [r1]   [d1]
    % [b2  b1           1  -2*cos(om)] * [s0] = [d2]
    % [ 0  b2           0           1]   [s1]   [d3]
    r0 = (2*d0*b1*cos(om)+2*d0*b2*cos(2*om)+d0*b2+d1*b1+2*d1*b2*cos(om)+b2*d2-b1*d3)/(b1^2+2*b1*b2*cos(om)+b2^2);
    r1 = (-d0*b1-2*d0*b2*cos(om)-b2*d1+b1*d2+2*d3*b1*cos(om)+d3*b2)/(b1^2+2*b1*b2*cos(om)+b2^2);
end;

param=[r0; r1; q0; q1; p0; p1];


function D = gsp_distanz(X, Y, P)
%GSP_DISTANZ calculates the distances between all vectors in X and Y
%   Usage: D = gsp_distanz(X, Y);
%
%   Input parameters:
%       X   : matrix with col vectors
%       Y   : matrix with col vectors (default == X)
%       P   : distance matrix (default Identity)
%   Output parameters:
%       D   : distance matrix, not squared
%
%   This code computes the following
%
%   .. D = ( (X-Y)^T P (X-Y) )^(0.5)
%
%   .. math:: D = \sqrt{(X-Y)^T P (X-Y)}
%
%   for all vectors in X an Y!
%
%   This code is not optimized for memory, but for speed because it uses no
%   loops.
%

% Testing: test_gsp_distanz

% Handle Input parameters
if nargin<1, error('Not enought inputs parameters!'); end
if nargin<2,  Y=X; end

% Get the size
[rx, cx] = size(X);
[ry, cy] = size(Y);

% Verify the size
if rx~=ry, error('The sizes of x and y do not fit!'), end

if nargin < 3
    xx = sum(X.*X,1); % ||x||^2
    yy = sum(Y.*Y,1); % ||y||^2
    xy = X'*Y;        % <y,x>
    % \|x-y\|^2 = ||x||^2 +||y||^2 - 2 <y,x>
    D = abs(repmat(xx',[1 cy]) + repmat(yy,[cx 1]) - 2*xy);
else

    [rp,rp2] = size(P);
    if rx~=rp, error('The sizes of x and P do not fit!'), end
    if rp2~=rp, error('P must be square!'), end

    xx = sum(X .* (P* X),1 ); % x^T P x
    yy = sum(Y .* (P* Y),1 ); % y^T P y
    xy = X'*(P*Y);        % x^T P y
    yx = Y'*(P*X);        % x^T P y

    D = abs(repmat(xx',[1 cy]) + repmat(yy,[cx 1]) - xy-yx);
end

if sum(D(:)<0)
    warning('gsp_distanz: P is not semipositive or x is not real!')
end

% Take the square root
D = sqrt(D);

if nargin < 2   % if Y == X
    % The diagonal has to be zero!
    D(1:cx+1:end) = 0;
end


function [alt, lat] = geodet1 (rmag, dec)

% geodetic latitude and altitude

% series solution

% input

%  rmag = geocentric radius (kilometers)
%  dec  = geocentric declination (radians)
%         (+north, -south; -pi/2 <= dec <= +pi/2)

% output

%  alt = geodetic altitude (kilometers)
%  lat = geodetic latitude (radians)
%        (+north, -south; -pi/2 <= lat <= +pi/2)

% global constants

%  req  = equatorial radius (kilometers)
%  flat = flattening factor (non-dimensional)

% reference: NASA TN D-7522

% Orbital Mechanics with Matlab

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global req flat

n = req / rmag;
o = flat * flat;

a = 2 * dec;
p = sin(a);
q = cos(a);

a = 4 * dec;
r = sin(a);
s = cos(a);

% geodetic latitude (radians)

lat = dec + flat * n * p + o * n * r * (n - 0.25);

% geodetic altitude (kilometers)

alt = rmag + req * (flat * 0.5 * (1.0 - q) + o * (0.25 * n - 0.0625) * (1.0 - s) - 1.0);


% GAMMA_LOGPDF - The log pdf of a Gamma distribution.
%
% Computes the logarithm of the Gamma probability density function
% G(ALPHA|A,B), where ALPHA is the random variable, A is the shape and B is
% the inverse scale (note the difference to MATLAB's built-in functions).
%
% The function is called as
%
% L = GAMMA_LOGPDF(ALPHA, A, B)
% L = GAMMA_LOGPDF(ALPHA, A, B, LOG_ALPHA)
%
% where LOG_ALPHA is LOG(ALPHA). Letting the user to evaluate this logarithm
% allows greater flexibility (e.g., for variational Bayesian cost function
% one can give the expectation of the logarithm). However, A and B are
% assumed to be fixed parameters in order to keep the function simple.

% Last modified 2010-06-07
% Copyright (c) Jaakko Luttinen

function [lp, dlp] = gamma_logpdf(alpha, a, b, log_alpha)

if nargin < 4
  log_alpha = log(alpha);
% $$$   if nargin == 4
% $$$     error(['Check the order of your input variables - the function has been ' ...
% $$$            'changed!'])
% $$$   end
% $$$ else
% $$$   error('Improper number of inputs')
end

lp = (a-1).*log_alpha - b.*alpha - gammaln(a) + a.*log(b);

if nargout >= 2
  inv_alpha = 1./alpha;
  dlp = (a-1).*inv_alpha - b;
end

%lp(alpha<=0) = -inf;
%dlp(alpha<=0) = nan;


%try_circ_fit
%
% IB
%
% revival of a 13 years old code

  % Create data for a circle + noise

  th = linspace(0,2*pi,20)';
  R=1.1111111;
  sigma = R/10;
  x = R*cos(th)+randn(size(th))*sigma;
  y = R*sin(th)+randn(size(th))*sigma;

   plot(x,y,'o'), title(' measured points')
   pause(1)

   % reconstruct circle from data
   [xc,yc,Re,a] = circfit(x,y);
      xe = Re*cos(th)+xc; ye = Re*sin(th)+yc;

     plot(x,y,'o',[xe;xe(1)],[ye;ye(1)],'-.',R*cos(th),R*sin(th)),
     title(' measured fitted and true circles')
      legend('measured','fitted','true')
      text(xc-R*0.9,yc,sprintf('center (%g , %g );  R=%g',xc,yc,Re))
     xlabel x, ylabel y
     axis equal


function a = p04_a ( m, n )

%*****************************************************************************80
%
%% P04_A returns the matrix A for problem 4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of equations.
%
%    Input, integer N, the number of variables.
%
%    Output, real A(M,N), the matrix.
%
  for j = 1 : n
    for i = 1 : m
      a(i,j) = j ^ ( i - 1 );
    end
  end

  return
end


function [ p, p2 ] = plot_diffusion_profiles( S, dash )
%UNTITLED5 Summary of this function goes here
%   Detailed explanation goes here

x = 0:1E-5:3E-3;

if nargin == 1
    dash = 0;
end

if dash
    dd = '--';
else
    dd = '-';
end

hold on;

if isfield(S, 'strexp')
    u = S.strexp.D;
    beta = S.strexp.gamma;

    k = 0:5000;

    tww = u^(1/beta);
    l = zeros(size(x));
    for t_counter = 1:length(x)

        t = x(t_counter);

        % see wikipedia page on stretched exponential function
        %tosum = ((-1).^k)./factorial(k).*sin(pi*beta*k).*gamma(beta*k+1).*(t/tww).^(beta*k+1);
        tosum = ((-1).^k).*sin(pi*beta*k).*exp(gammaln(beta*k + 1) - gammaln(k + 1) + (beta*k+1)*log(t/tww));
        tosum(isnan(tosum)) = 0;

        l(t_counter) = -tww/(pi*t)*sum(tosum);
    end
    Y = l./x;
    Y(x < 0) = 0;
    Y(Y <= 0) = 0;
    Y(x > 2.5E-3) = 0; % to avoid numerical errors
    plot(x*1000, Y, dd, 'Color', [0.5 0.5 0.5], 'LineWidth', 2)

end

if isfield(S, 'DKI')
    D = S.DKI.D;
    K = S.DKI.K;

    sigma = sqrt(1/3 * D^2 * K);
    Y = normpdf(x, D, sigma);

    plot(x*1000, Y, [dd 'g'], 'LineWidth', 2);

end

if isfield(S, 'stat')
    mu = S.stat.mu;
    sigma = S.stat.sigma;
    if isfield(S.stat, 'Dmin')
        Dmin = S.stat.Dmin;
    else
        Dmin = 0;
    end
    if isfield(S.stat, 'Dmax')
        Dmax = S.stat.Dmax;
    else
        Dmax = inf;
    end

    Y = normpdf(x, mu, sigma) ...
      / (normcdf(Dmax, mu, sigma) - normcdf(Dmin, mu, sigma));
    Y(x < Dmin) = 0;
    Y(x > Dmax) = 0;

    plot(x*1000, Y, [dd 'k'], 'LineWidth', 2);

end

if isfield(S, 'gamma')
%     D = S.gamma.D;
%     sigma = S.gamma.sigma;
%     kappa = D^2 / sigma^2;
%     theta = sigma^2 / D;

    kappa = S.gamma.kappa;
    theta = S.gamma.theta;

    Ygamma = x.^(kappa - 1) .* exp(- x / theta) ./ (gamma(kappa) * theta^kappa);
    Ygamma(x < 0) = 0;

    plot(x*1000, Ygamma, dd, 'Color', [1 0.69 0.39], 'LineWidth', 2);

end

if 0%isfield(S, 'doubletrunc')

    mu = S.doubletrunc.mu;
    sigma = S.doubletrunc.sigma;
    D_upper = S.doubletrunc.D_upper;

    Y = normpdf(x, mu, sigma) / (normcdf(D_upper, mu, sigma) - normcdf(0, mu, sigma));
    Y(x < 0) = 0;
    Y(x > D_upper) = 0;

    plot(x*1000, Y, 'k--', 'LineWidth', 2);

end

if isfield(S, 'beta')
    a = S.beta.alpha;
    b = S.beta.beta;
    D0 = S.beta.D0;

    P = 1/D0 * gamma(a + b) / (gamma(a) * gamma(b)) *  (x/D0).^(a - 1) .* (1 - x / D0).^(b - 1);
    P(x < 0) = 0;
    P(x > D0) = 0;

    p = plot(x*1000, real(P), [dd 'r'], 'LineWidth', 2);
end

if isfield(S, 'monoexp')
    D = S.monoexp;
    plot([D D]*1000, [0 1500], dd, 'LineWidth', 2);

end

if isfield(S, 'biexp')
    Dfast = S.biexp.Dfast;
    Dslow = S.biexp.Dslow;
    f = S.biexp.f;
    dd = '-';
    p2 = plot([Dfast Dfast NaN Dslow Dslow]*1000, [0 1500*f NaN 0 1500*(1-f)], [dd 'c'], 'LineWidth', 2);

end

%legend('Mono-exponential', 'Bi-exponential', 'DKI (Gaussian)', 'Truncated Gaussian', 'Gamma', 'Beta')

set(gca, 'FontSize', 30)
set(gca, 'LineWidth', 2)
set(gca, 'XLim', [0 3])


disp('Test: Mutual information between two images')
load mri
A=D(:,:,8);
B=D(:,:,9);
mi(A,A),mi(A,B)

disp('Test: Mutual information between two signals')
load garchdata
nasdaq = price2ret(NASDAQ);
nyse = price2ret(NYSE);
mi(nasdaq,nasdaq), mi(nasdaq,nyse)


%%********************************************************************
%% This is an example for the interface
%% This example is to solve the following SDP
%%
%% min trace(X1)+trace(X2)+sum(X3)
%% s.t. -X1(1,2)+2X2(3,3)+2X3(2)=4
%%       2X1(2,3)+X2(4,2)-X3(4) = 3
%%       2<= -X1(1,2)-2X2(3,3)+2X3(2)<=7
%%       X1 \in S^{6}_{+}, X2 \in R^{5*5}, X3 \in R^{7}_{+}
%%       0<=X1<=10, 0<=X2<=8
%% SDPNAL+:
%% Copyright (c) 2017 by
%% Yancheng Yuan, Kim-Chuan Toh, Defeng Sun and Xinyuan Zhao
%%********************************************************************
clear all;
n1 = 6; n2 = 5; n3 = 7;
%% Initialize model
model = ccp_model('Example_simple');
    X1 = var_sdp(n1,n1);
    X2 = var_nn(n2, n2);
    X3 = var_nn(n3);
    model.add_variable(X1, X2, X3);
    model.minimize(trace(X1)+trace(X2)+sum(X3));
    options=3;
    if (options==1)
       model.add_affine_constraint(-X1(1,2)+2*X2(3,3)+2*X3(2)==4);
       model.add_affine_constraint(2*X1(2,3)+X2(4,2)-X3(4) == 3);
    else
       %%two other ways to add the above equality constraints
       A1 = {sparse(n1,n1), sparse(n1,n1)};
       A2 = {sparse(n2,n2), sparse(n2,n2)}; A3 = sparse(2,n3);
       A1{1}(1,2)= -1;  A1{2}(2,3)= 2;
       A2{1}(3,3)= 2;   A2{2}(4,2)= 1;
       A3(1,2) = 2;     A3(2,4) = -1;
       b = [4;3];
       if (options==2)
          model.add_affine_constraint(A1*X1+A2*X2+A3*X3 == b);
       elseif (options==3)
          blk{1,1} = 's'; blk{1,2} = n1;
          %A1vec = [svec(blk,A1{1}),svec(blk,A1{2})]';
          A1vec = [A1{1}(:), A1{2}(:)]';
          %model.add_affine_constraint(A1vec*map_svec(X1)+A2*X2+A3*X3 == b);
          model.add_affine_constraint(A1vec*map_vec(X1)+A2*X2+A3*X3 == b);
       end
    end
    model.add_affine_constraint(2<=-X1(1,2)-2*X2(3,3)+2*X3(2)<=7);
    model.add_affine_constraint(0<= X1 <=10);
    model.add_affine_constraint(X2<=8);
    model.add_affine_constraint(map_diag(X1)==ones(n1,1));
    model.setparameter('BBTsolve.method','direct');
model.solve;
%%*********************************************************************


% Computation of the S_Method
% Usage:
%
%       output = S_method( input1, L, wl, win, param_overlap, fftl);
%
%
% Parameters:
%
%    signal
%
%         Input one dimensional signal to be analysed. An analytic signal
%         is required for this function, however, if signal is real, a
%         default analytic transformer routine will be called from this
%         function before computing spectrogram.
%
%    L
%
%        Define the length of frequency window in the S-Method (2*L+1)
%
%    window_length
%
%         Length of choosen window.
%
%
%    window_type
%
%         One of  'rect', 'hann', 'hamm', 'bart'
%
%   overlap
%
%         Number of samples in common between two consecutives windows.
%         0 < overlap < window_length.
%
%    fft_length
%
%         Zero-padding at the fft stage of the analysis may be specified by
%         giving an fft_length larger than normal. If fft_length is not
%         specified then the next highest power of two above the
%         signal length will be used. If fft_length is not a power
%         of two, then the next highest power of two is used.
%
%
%    tfd
%
%        The computed time-frequency distribution. size(tfd) will
%        return [a/2+1, b], where a is the next largest power of two above
%        window_length, and b is ceil((length(signal)-overlap)/(window_length-overlap)) .
%
%
%
%  See Also: spec
% TFSAP 7.0
% Copyright Prof. B. Boashash
% Qatar University, Doha
% email: tfsap.research@gmail.com


function quadrule_test28 ( )

%*****************************************************************************80
%
%% TEST28 tests LEGENDRE_SET_X0_01 and SUM_SUB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2009
%
%  Author:
%
%    John Burkardt
%
  order_max = 8;

  nfunc = func_set ( 'COUNT', 'DUMMY' );

  a = 0.0;
  b = 1.0;

  nsub = 1;

  xlo =  0.0;
  xhi =  1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST28\n' );
  fprintf ( 1, '  LEGENDRE_SET_X0_01 sets up Gauss-Legendre quadrature\n' );
  fprintf ( 1, '    for integrating F(X) over [0,1];\n' );
  fprintf ( 1, '  SUM_SUB carries it out.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The integration interval is [%f, %f]\n', a, b );
  fprintf ( 1, '  Number of subintervals is %d\n', nsub );
  fprintf ( 1, '  Quadrature order will vary.\n' );
  fprintf ( 1, '  Integrand will vary.\n' );
  fprintf ( 1, '\n' );

  for ilo = 1 : 5 : nfunc

    ihi = min ( ilo + 4, nfunc );

    fprintf ( 1, '\n' );
    fprintf ( 1, '    ' );
    for i = ilo : ihi
      fprintf ( '%14s', fname(i) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '\n' );

    for norder = 1 : order_max

      fprintf ( 1, '  %2d', norder );

      for i = ilo : ihi

        func_set ( 'SET', i );

        [ xtab, weight ] = legendre_set_x0_01 ( norder );

        result(i) = sum_sub ( @func, a, b, nsub, norder, xlo, xhi, ...
          xtab, weight );

        fprintf ( 1, '  %12f', result(i) );

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


% VL_PEGASOS PEGASOS linear SVM solver
%   W = VL_PEGASOS(X, Y, LAMBDA) learns a linear SVM W given training
%   vectors X, their labels Y, and the regularization parameter LAMBDA
%   using the PEGASOS [1] solver. The algorithm finds a minimizer W of
%   the objective function
%
%     LAMBDA/2 |W|^2 + 1/N SUM_i LOSS(W, X(:,i), Y(i))
%
%   where LOSS(W,X,Y) = MAX(0, 1 - Y W'X) is the hinge loss and N is
%   the number of training vectors in X.
%
%   ALGORITHM. PEGASOS is an implementation of stochastic subgradient
%   descent. At each iteration a data point is selected at random, the
%   subgradient of the cost function relative to that data point is
%   computed, and a step is taken in that direction. The step size is
%   inversely proportional to the iteration number. See [1] for
%   details.
%
%   NumIterations:: [10 / LAMBDA]
%     Sets the maximum number of iterations.
%
%   BiasMultiplier:: [0]
%     Appends to the data X the specified scalar value B. This
%     approximates the training of a linear SVM with bias.  The bias
%     can be recovered from the optimal weight vector W as W(end) *
%     B. It is often useful to precondition the computation of the
%     gradient with respect to this element by a small value,
%     e.g. 0.1/B (see PRECONDITIONER).
%
%   StartingModel:: [null vector]
%     Specify the initial value for the weight vector W.
%
%   StartingIteration:: [1]
%     Specify the iteration number to start from. The only effect
%     is to change the step size, as this is inversely proportional
%     to the iteration number.
%
%   Permutation:: [empty]
%     Specify a permutation PERM to be used to sample the data (this
%     disables random sampling). Specifically, at the T-th iteration
%     the algorithm takes a step w.r.t. the PERM[T']-th data point,
%     where T' is T modulo the number of data samples
%     (i.e. MOD(T'-1,NUMSAMPLES)+1). PERM needs not to be
%     bijective. This allows specifying certain data points more or
%     less frequently, implicitly increasing their relative weight in
%     the error term. A common application is to balance an unbalanced
%     dataset.
%
%   Preconditioner:: [empty]
%     Specify a diagonal preconditioner PREC. The elements of this
%     vector are multiplied to the function subgradient before adding
%     the latter to the current model estimate. The dimension of the
%     vector PREC is the same of the model, plus one if the SVM has
%     bias (see BIASMULTIPLIER).
%
%   Verbose::
%     Be verbose.
%
%   Example::
%     The options StartingModel and StartingIteration can be used
%     to continue training. I.e., the command
%
%       vl_twister('state',0) ;
%       w = vl_pegasos(x,y,lambda,'NumIterations',1000) ;
%
%     produces the same result as the sequence
%
%       vl_twister('state',0) ;
%       w = vl_pegasos(x,y,lambda,'NumIterations',500) ;
%       w = vl_pegasos(x,y,lambda,'NumIterations',500, ...
%                      'StartingIteration', 501, ...
%                      'StartingModel', w) ;
%
%   REFERENCES::
%     [1] S. Shalev-Shwartz, Y. Singer, N. Srebro, and
%     A. Cotter. Pegasos: Primal Estimated sub-GrAdient SOlver for
%     SVM. MBP, 2010.
%
%   See also: VL_HOMKERMAP(), VL_HELP().

% AUTHORIGHTS


function monogrid_poisson_1d_test01_multi ( )

%*****************************************************************************80
%
%% MONOGRID_POISSON_1D_TEST01_MULTI tests MULTIGRID_POISSON_1D on test case 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 December 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MONOGRID_POISSON_1D_TEST01_MULTI\n' );
  fprintf ( 1, '  MULTIGRID_POISSON_1D solves a 1D Poisson BVP\n' );
  fprintf ( 1, '  using the multigrid method.\n' );

  a = 0.0;
  b = 1.0;
  ua = 0.0;
  ub = 0.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  -u"(x) = 1, for %g < x < %g\n', a, b );
  fprintf ( 1, '  u(%g) = %g, u(%g) = %g.\n', a, ua, b, ub );
  fprintf ( 1, '  Solution is u(x) = ( -x^2 + x ) / 2\n' );

  for k = 5 : 5

    n = 2^k;
    x = ( linspace ( a, b, n + 1 ) )';

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Mesh index K = %d\n', k );
    fprintf ( 1, '  Number of intervals N=2^K = %d\n', n );
    fprintf ( 1, '  Number of nodes = 2^K+1 =   %d\n', n + 1 );

    [ u, it_num ] = multigrid_poisson_1d ( n, a, b, ua, ub, @force1, @exact1 );

    fprintf ( 1, '\n' );
    fprintf ( 1, '     I        X(I)      U(I)         U Exact(X(I))\n' );
    fprintf ( 1, '\n' );
    for i = 1 : n + 1
      fprintf ( 1, '  %4d  %10f  %14g  %14g\n', i, x(i), u(i), exact1 ( x(i) ) );
    end

    fprintf ( 1, '\n' );

    difmax = 0.0;
    for i = 1 : n + 1
      difmax = max ( difmax, abs ( u(i) - exact1 ( x(i) ) ) );
    end
    fprintf ( 1, '  Maximum error = %g\n', difmax );
    fprintf ( 1, '  Number of iterations = %d\n', it_num );

  end

  return
end


function [eigvec, eigval] = PTKVectorisedEigenvalues(M, eigenvalues_only)
    % PTKVectorisedEigenvalues. Computes eigenvalues and eigenvectors for many symmetric matrices
    %
    %     PTKVectorisedEigenvalues is similar to Matlab's eigs() function, but can be
    %     used to compute the eigenvalues and eigenvectors for multiple matrices,
    %     which for a large number of points is significantly quicker than using a
    %     for loop. Each input matrix must be symmetric and is represented by a
    %     single row of the input matrix as described below.
    %
    %     The mex function PTKFastEigenvalues is equivalent to function but runs
    %     faster. PTKVectorisedEigenvalues is slower than PTKFastEigenvalues but still
    %     significantly faster than running eigs() in a for loop when a large
    %     number of matrices is involved.
    %
    %
    %         Syntax:
    %             [eigvectors, eigvalues] = PTKFastEigenvalues(M [, eigenvalues_only])
    %
    %         Input:
    %             M is a 6xn matrix. Each column of M represents one 3x3 symmetric matrix as follows
    %
    %                     [V(1) V(2) V(3); V(2) V(4) V(5); V(3) V(5) V(6)]
    %
    %                 where V is a 6x1 column of M
    %
    %             eigenvalues_only is an optional parameter which defaults to
    %                 false. Set to true to only calculate eigenvalues and not
    %                 eigenvectors, which reduces the execution time.
    %
    %          Outputs:
    %              eigenvalues is a 3xn matrix. Each column contains the 3
    %                  eigenvalues of the matrix V described above
    %
    %              eigenvectors is a 3x3xn matrix, where each 3x1 row represents
    %                  an eigenvector (3 for each of the n matrices V described
    %                  above).
    %
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    compute_eigenvectors = true;
    if exist('eigenvalues_only', 'var')
        if (eigenvalues_only)
            compute_eigenvectors = false;
        end
    end

    numvoxels = size(M, 2);
    eigval = zeros(3, numvoxels, 'double');

    m = (M(1,:) + M(4,:) + M(6,:))/3;

    q =  (( M(1,:) - m) .* (M(4,:) - m) .* (M(6,:) - m) + ...
        2 * M(2,:) .* M(5,:) .* M(3,:) - ...
        M(3,:).^2 .* (M(4,:) - m) - ...
        M(5,:).^2 .* (M(1,:) - m) - M(2,:).^2 .* (M(6,:) - m) )/2;

    p = ( ( M(1,:) - m ).^2 + 2 * M(2,:).^2 + 2 * M(3,:).^2 + ...
        ( M(4,:) - m ).^2 + 2 * M(5,:).^2 + ( M(6,:) - m ).^2 )/6;

    p = max(0.01, p);

    phi = 1/3*acos(min(1, q./p.^(3/2)));
    phi(phi<0) = phi(phi<0)+pi/3;

    eigval(1,:) = m + 2*sqrt(p).*cos(phi);
    eigval(2,:) = m - sqrt(p).*(cos(phi) + sqrt(3).*sin(phi));
    eigval(3,:) = m - sqrt(p).*(cos(phi) - sqrt(3).*sin(phi));

    [~, i] = sort(abs(eigval));
    i = i + size(eigval,1)*ones(size(eigval,1), 1)*(0:size(eigval, 2) - 1);
    eigval = eigval(i);

    % Only compute the eigenvectors if requested
    if (compute_eigenvectors)

        eigvec = zeros(3,3, numvoxels, 'double');
        for l = 1 : 2
            Ai = M(1,:) - eigval(l,:);
            Bi = M(4,:) - eigval(l,:);
            Ci = M(6,:) - eigval(l,:);

            eix = ( M(2,:) .* M(5,:) - Bi .* M(3,:) ) .* ( M(3,:) .* M(5,:) - Ci .* M(2,:) );
            eiy = ( M(3,:) .* M(5,:) - Ci .* M(2,:) ) .* ( M(3,:) .* M(2,:) - Ai .* M(5,:) );
            eiz = ( M(2,:) .* M(5,:) - Bi .* M(3,:) ) .* ( M(3,:) .* M(2,:) - Ai .* M(5,:) );

            vec = sqrt(eix.^2+eiy.^2+eiz.^2);
            vec = max(0.01, vec);
            eigvec(:,l,:) = [eix; eiy; eiz] ./ vec([1;1;1], :);
        end

        eigvec(:,3,:) = cross(eigvec(:,1,:), eigvec(:,2,:));
    else
        eigvec = [];
    end
end