function  [Z] =  MCWNNM_ADMM( Y, NSig, Par )
% This routine solves the following weighted nuclear norm optimization problem with column weights,
%
% min_{X, Z} ||W(Y-X)||_F^2 + ||Z||_w,*  s.t.  X = Z
%
% Inputs:
%        Y      -- 3p^2 x M dimensional noisy matrix, D is the data dimension, and N is the number of image patches.
%        NSig -- 3p^2 x 1 dimensional vector of weights
%        Par   -- structure of parameters
% Output:
%        Z      -- 3p^2 x M dimensional denoised matrix

% tol = 1e-8;
if ~isfield(Par, 'maxIter')
    Par.maxIter = 10;
end
if ~isfield(Par, 'rho')
    Par.rho = 1;
end
if ~isfield(Par, 'mu')
    Par.mu = 1;
end
if ~isfield(Par, 'display')
    Par.display = true;
end
%% Initializing optimization variables
% intialize
mNSig = min(NSig);
W = (mNSig+eps) ./ (NSig+eps);

Z = zeros(size(Y));
A = zeros(size(Y));
%% Start main loop
iter = 0;
PatNum       = size(Y,2);
TempC  = Par.Constant * sqrt(PatNum) * mNSig^2;
while iter < Par.maxIter
    iter = iter + 1;

    % update X, fix Z and A
    % min_{X} ||W * Y - W * X||_F^2 + 0.5 * rho * ||X - Z + 1/rho * A||_F^2
    X = diag(1 ./ (W.^2 + 0.5 * Par.rho)) * (diag(W.^2) * Y + 0.5 * Par.rho * Z - 0.5 * A);

    % update Z, fix X and A
    % min_{Z} ||Z||_*,w + 0.5 * rho * ||Z - (X + 1/rho * A)||_F^2
    Temp = X + A/Par.rho;
    [U, SigmaTemp, V] =   svd(full(Temp), 'econ');
    [SigmaZ, svp] = ClosedWNNM(diag(SigmaTemp), 2/Par.rho*TempC, eps);
    Z =  U(:, 1:svp) * diag(SigmaZ) * V(:, 1:svp)';
    %     % check the convergence conditions
    %     stopC = max(max(abs(X - Z)));
    %     if Par.display && (iter==1 || mod(iter,10)==0 || stopC<tol)
    %         disp(['iter ' num2str(iter) ',mu=' num2str(Par.mu,'%2.1e') ...
    %             ',rank=' num2str(rank(Z,1e-4*norm(Z,2))) ',stopALM=' num2str(stopC,'%2.3e')]);
    %     end
    %     if stopC < tol
    %         break;
    %     else
    % update the multiplier A, fix Z and X
    A = A + Par.rho * (X - Z);
    Par.rho = min(1e4, Par.mu * Par.rho);
    %     end
end
return;


function [loopcheck, pc_, pp_, pk_, sdc, sdp, sdk] = ploop_c_and_p_calcs(MIN_CSTEP, MIN_PSTEP, trackLoop, stdevcall)
    % loop for calculating c and p values
    % if cstep is empty, then neither cstep or err1 will be calculated
    % sdevcall will contain 'kpc', 'kp', or 'kp~' to determine how kcp_stdevs behaves
    % -  'kpc' will calculate the k, p, and c errors
    % - 'kp' calculates only the k & p errors
    % - 'kp~' also calculates the k & p errors, but in a 3x matrix
    %   where the results for c are ignored
    % all these options exist because that is how they were
    % implemented by the ploop functions

    % modified omori law:
    % n(t) = K /(t+c)^p
    % n(t) : frequency of aftershocks per unit time interval,
    % K : the productivity of the sequence,
    % c : adjusts for missing earthquakes in the catalog
    % p : how quickly the activity falls off to the constant background intensity.
    % typically normaized to days
    %
    % ploop parts attributed to A.Allmann and B. Enescu
    % routines deconstructed & merged by C. Reyes 2017

    % TODO: get rid of all these globals!!@!!Q#RQR@!#R$@!
    global pc pp pk
    global tt loop nn
    global nit t
    global err1x err2x  % previous errors
    global ieflag isflag %error small enough, step small enough
    global qp
    global err1 err2
    global cstep pstep
    global ts eps1 eps2
    global pcheck
    %global sdk sdp sdc

    loopcheck=0;

    % pp = initial p = 1.1
    % nn = length of catalog (# of events)
    % tt = end time (last event)
    % ts = start time (1st event)
    % pc = initial c???
    % t  = time of each event (a vector)
    while loopcheck < 500
        if ~isnumeric(t)
            %fprintf('t is a %s',class(t));
            % normalize all times to days after first event
            t = days(t - ts);
            tt = days(tt - ts);
            ts = days(ts - ts);
            %fprintf(' now t is a %s\n',class(t) );
        end

        % preverr was never assigned anywhere
        preverr = 0.0;

        if pp==1.0
            pp=1.001;
        end

        if trackLoop
            loop=loop+1;
        end
        loopcheck=loopcheck+1;
        nit=nit+1;

        qp=1-pp;

        pk=(qp*nn)/((tt+pc)^qp-(ts+pc)^qp);

        update_cof_and_cog(pk, qp, ts, pc)

        if ~isempty(MIN_CSTEP) % kludgy! but still reduces lines tremendously
            err1 = c_err(pk, pc, t, ts, tt, pp);
            ieflag = ieflag || abs(err1)< eps1 ;
            isflag = isflag || cstep <=MIN_CSTEP;
        end
        err2 = p_err(t, pc, pk, qp, tt, ts);

        ieflag = ieflag || ( abs(err2) <  eps2); % errors small enough
        isflag = isflag || ( pstep <= MIN_PSTEP); % steps small enough

        %stop searching if errors or steps are small enough
        if ieflag || isflag
            switch stdevcall
                case 'kpc'
                    [sdk, sdp, sdc] = kcp_stdevs();
                case 'kp'
                    [sdk, sdp] = kcp_stdevs();
                case 'kp~'
                    [sdk, sdp, ~] = kcp_stdevs();
            end
            pcheck = 1;
            break
        else
            if trackLoop
                [cstep, pc, loop] = take_cstep(MIN_CSTEP, cstep, preverr, ts, pc, nit, err1, loop);
            else
                [cstep, pc, ~] = take_cstep(MIN_CSTEP, cstep, preverr, ts, pc, nit, err1, []);
            end
            [pstep, pp] = take_pstep(MIN_PSTEP, pp, pstep, preverr, nit, err2);
        end

        if ~isempty(MIN_CSTEP)
            err1x=err1;
        end
        err2x=err2;

    end

    %p= pp;
    pp_=pp;
    pc_=pc;
    pk_=pk;

end

%%
function cerr = c_err(pk, pc, t, ts, tt, pp)
    %c_err calculate c error
    qsum=pk*((1/(tt+pc)^pp)-(1/(ts+pc)^pp));
    psum=sum(1./(t+pc));
    cerr=qsum+pp*psum;
end

function perr = p_err(t, pc, pk, qp, tt, ts)
    %p_err calculate p error
    sumln=sum(log(t+pc));
    qsumln=pk/qp^2;
    qsumln=qsumln*(((tt+pc)^qp)*(1-qp*log(tt+pc))-((ts+pc)^qp)*(1-qp*log(ts+pc)));
    esumln=qsumln+sumln;
    perr=esumln;
end

function [cstep, pc, loop] = take_cstep(MIN_CSTEP, cstep, preverr, ts, pc, nit, err1, loop)
    if isempty(MIN_CSTEP); return; end

    STEP_REDUCTIONFACTOR = 0.9;

    if nit>1
        %if error has changed sign,reduce the step size
        if ~same_sign(preverr,err1)  &&  cstep >= MIN_CSTEP
            cstep=cstep * STEP_REDUCTIONFACTOR;
        end
    end

    pc = pc - cstep * sign(err1); % move closer to zero

    dt=ts+pc;
    if dt <= 0
        pc=ts+cstep;
    end
    if ~isempty(loop) && loop == 30
        cstep=cstep * STEP_REDUCTIONFACTOR;
        loop=0;
    end

end

function [pstep, pp] = take_pstep(MIN_PSTEP, pp, pstep, preverr, nit, err2)
    % calculate the parameters of p-value
    if isempty(MIN_PSTEP); return; end
    STEP_REDUCTIONFACTOR = 0.9;

    if nit>1
        %if error has changed sign,reduce the step size
        if ~same_sign(preverr,err2) &&  pstep>= MIN_PSTEP
            pstep=pstep * STEP_REDUCTIONFACTOR;
        end
    end

    pp = pp - pstep * sign(err2); % move closer to zero
end

function [sdk, sdp, sdc] = kcp_stdevs()
    % calculate standard deviations for k, c, and p values
    %kcp_stdevs.m                          A.Allmann
    %
    % calculat the parameters of p-value
    %calls itself with different parameters for different loops in programm

    % TODO: get rid of all these globals!!@!!Q#RQR@!#R$@!

    global p c tt pc pp
    global pk ts dk

    te=tt;
    p=pp;
    c=pc;
    dk=pk;

    %case1
    f1=((te+c)^(-p+1))/(-p+1);
    h1=((ts+c)^(-p+1))/(-p+1);
    s(1)=(1/dk)*(f1-h1);

    %case2
    f2=((te+c)^(-p));
    h2=((ts+c)^(-p));
    s(2)=f2-h2;

    %case3

    f3=(-(te+c)^(-p+1))*(((log(te+c))/(-p+1))-(1/((-p+1)^2)));
    h3=(-(ts+c)^(-p+1))*(((log(ts+c))/(-p+1))-(1/((-p+1)^2)));
    s(3)=f3-h3;

    %case4

    s(4)=s(2);

    %case5

    f5=((te+c)^(-p-1))/(p+1);
    h5=((ts+c)^(-p-1))/(p+1);
    s(5)=(-dk)*(p^2)*(f5-h5);

    %case6

    f6=((te+c)^(-p))*(((log(te+c))/(-p))-(1/(p^2)));
    h6=((ts+c)^(-p))*(((log(ts+c))/(-p))-(1/(p^2)));
    s(6)=(dk*p)*(f6-h6);

    %case7

    s(7)=s(3);

    %case8

    s(8)=s(6);

    %case9

    f10=((te+c)^(-p+1))*((log(te+c))^2)/(-p+1);
    f11=(2*((te+c)^(-p+1)))/((-p+1)^2);
    f12=(log(te+c))-(1/(-p+1));
    f9=f10-(f11*f12);

    h10=((ts+c)^(-p+1))*((log(ts+c))^2)/(-p+1);
    h11=(2*((ts+c)^(-p+1)))/((-p+1)^2);
    h12=(log(ts+c))-(1/(-p+1));
    h9=h10-(h11*h12);
    s(9)=(dk)*(f9-h9);

    %assign the values of s to the matrix A(i,j)
    %invert the matrix to calculate the standard deviation
    %for k,c,p .

    if nargout == 3
        A=[s(1) s(2) s(3); s(4) s(5) s(6); s(7) s(8) s(9)];

        A=inv(A);

        sdk=sqrt(A(1,1));
        sdc=sqrt(A(2,2));
        sdp=sqrt(A(3,3));

    elseif nargout == 2
        A=[s(1) s(3); s(3) s(9)];
        A=inv(A);
        sdk=sqrt(A(1,1));
        sdp=sqrt(A(2,2));
    else
        error('wrong number of output arguments');
    end

end


function [output] = conv3d(input, w, b, stride, pad)
% input: Ci x D x H x W
% w: Co x Ci x D x H x W
% b: Co
% stride: int
% pad: int

assert(length(size(input)) == 4);
assert(length(size(w)) == 5 || length(size(w)) == 2);
assert(size(input,2) == size(input,3));
assert(size(input,3) == size(input,4));
assert(size(w,2) == size(input,1));

% padding
input = padarray(input, [0, pad, pad, pad]);

S = stride;
if length(size(w)) == 2 % for cccp kernel
    A = 1;
else
    A = size(w,3);
end
C_out = size(w,1);

% create tensor for output
n = floor((size(input,2)-A) / stride + 1);
output = zeros(size(w,1),n,n,n);

% 3d convolution
for i = 1:n
    for j = 1:n
        for k = 1:n
            patch = input(:,(i-1)*S+1:(i-1)*S+A,...
                    (j-1)*S+1:(j-1)*S+A,...
                    (k-1)*S+1:(k-1)*S+A);
              output(:,i,j,k) = sum(repmat(reshape(patch,1,[]),C_out,1).* ...
                  reshape(w,C_out,[]),2) + reshape(b,[],1);
        end
    end
end

end


%% Ex. 2 For loop: Utility of the dummy index

b = 3;
for k = 1:5
 b^k
end
%Output:
% 3
% 9
% 27
% 81
% 243

%Remark: The outputs are 3^1, 3^2, 3^3, 3^4, and 3^5. the value of "k" keeps changing as
% we go through the loop


function [frq,cr] = v_cent2frq(c)
%V_FRQ2ERB  Convert Hertz to Cents frequency scale [C,CR]=(FRQ)
%	frq = v_frq2mel(c) converts a vector of frequencies in cents
%	to the corresponding values in Hertz.
%   100 cents corresponds to one semitone and 440Hz corresponds to 5700
%   cents.
%   The optional cr output gives the gradient in Hz/cent.
%
%	The relationship between cents and frq is given by:
%
%	c = 1200 * log2(f/(440*(2^((3/12)-5)))
%
%	Reference:
%
%     [1] Ellis, A.
%         On the Musical Scales of Various Nations
%         Journal of the Society of Arts, 1885, 485-527

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_cent2frq.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent p q
if isempty(p)
    p=1200/log(2);
    q=5700-p*log(440);
end
% c = 1200*sign(frq).*log2(frq/(440*2^((3/12)-5)));
af=(exp((abs(c)-q)/p));
frq=sign(c).*af;
cr=af/p;
if ~nargout
    plot(c,frq,'-x');
    ylabel(['Frequency (' v_yticksi 'Hz)']);
    xlabel(['Frequency (' v_xticksi 'Cents)']);
end


% Maximize stopband attenuation of a lowpass IIR filter
% "Linear Programming Design of IIR Digital Filters with Arbitrary
%  Magnitude Functions" by L.R. Rabiner, N.Y. Graham, and H.D. Helms
% (figures are generated)
%
% Designs a lowpass IIR filter using spectral factorization method where we:
% - minimize maximum stopband attenuation
% - have a constraint on the maximum passband ripple
%
%   minimize   max |H(w)|                      for w in the stopband
%       s.t.   1/delta <= |H(w)| <= delta      for w in the passband
%
% where we now have a rational frequency response function:
%
%   H(w) = sum_{m=0}^{M-1} b_m exp{-jwm} / sum_{n=1}^{N-1} a_n exp{-jwn}
%
% We change variables via spectral factorization method and get:
%
%   minimize   max R(w)                        for w in the stopband
%       s.t.   (1/delta)^2 <= R(w) <= delta^2  for w in the passband
%              R(w) >= 0                       for all w
%
% where R(w) is the squared magnited of the frequency response
% (and the Fourier transform of the autocorrelation coefficients r).
% We represent R(w) = N_hat(w)/D_hat(w), where now R(w) is a rational
% function since we deal with IIR filter (see the reference paper).
%
% Variables are coeffients of the numerator, denoted as c, and
% denominator, denoted as d. delta is the allowed passband ripple.
% This is a quasiconvex problem and can be solved using bisection.
%
% Written for CVX by Almir Mutapcic 02/02/06

%*********************************************************************
% user's filter specs (for a low-pass filter example)
%*********************************************************************
% number of coefficients for the IIR filter (including the zeroth one)
% (also without loss of generality we can assume that d_0 = 1, which
%  is the zeroth coefficient of the autocorrelation denominator)
M = 4; % nominator
N = 4; % denominator

wpass = 0.12*pi;   % end of the passband
wstop = 0.24*pi;   % start of the stopband
delta = 1;         % maximum passband ripple in dB (+/- around 0 dB)

%*********************************************************************
% create optimization parameters
%*********************************************************************
% rule-of-thumb discretization (from Cheney's Approx. Theory book)
sample_order = 30;
m = 15*(sample_order);
w = linspace(0,pi,m)'; % omega

% A's are matrices used to compute the power spectrum
Anum = [ones(m,1) 2*cos(kron(w,[1:M-1]))];
Aden = [ones(m,1) 2*cos(kron(w,[1:N-1]))];

% passband 0 <= w <= w_pass
ind = find((0 <= w) & (w <= wpass));    % passband
Ap_num  = Anum(ind,:);
Ap_den  = Aden(ind,:);

% transition band is not constrained (w_pass <= w <= w_stop)

% stopband (w_stop <= w)
ind = find((wstop <= w) & (w <= pi));   % stopband
As_num  = Anum(ind,:);
As_den  = Aden(ind,:);

%********************************************************************
% optimization
%********************************************************************

% use bisection (on the log of vars) to solve for the min stopband atten
Us_top = 1e-0; % 0 dB
Us_bot = 1e-6; % -60 dB (in original variables)

while( 20*log10(Us_top/Us_bot) > 1)
  % try to find a feasible design for given specs
  Us_cur = sqrt(Us_top*Us_bot);

  % formulate and solve the magnitude design problem
  cvx_begin quiet
    variable c(M,1)
    variable d(N-1,1)

    % feasibility problem
    % passband constraints
    (Ap_num*c) <= (10^(+delta/20))^2*(Ap_den*[1;d]); % upper constr
    (Ap_num*c) >= (10^(-delta/20))^2*(Ap_den*[1;d]); % lower constr
    % stopband constraint
    (As_num*c) <= (Us_cur)*(As_den*[1;d]); % upper constr
    % nonnegative-real constraint
    Anum*c >= 0;
    Aden*[1;d] >= 0;
  cvx_end

  % bisection
  if ~any(isnan(c)) % feasible
    fprintf(1,'Problem is feasible for stopband atten = %3.2f dB\n', ...
               10*log10(Us_cur));
    Us_top = Us_cur;
    b = spectral_fact(c);
    a = spectral_fact([1;d]);
  else % not feasible
    fprintf(1,'Problem not feasible for stopband atten = %3.2f dB\n', ...
               10*log10(Us_cur));
    Us_bot = Us_cur;
  end
end

% display the max attenuation in the stopband (convert to original vars)
fprintf(1,'\nOptimum min stopband atten is between %3.2f and %3.2f dB.\n',...
        10*log10(Us_bot),10*log10(Us_top));
disp('Optimal IIR filter coefficients are: ')
disp('Numerator: '), b
disp('Denominator: '), a

%*********************************************************************
% plotting routines
%*********************************************************************
% frequency response of the designed filter, where j = sqrt(-1)
H = ([exp(-j*kron(w,[0:M-1]))]*b)./([exp(-j*kron(w,[0:N-1]))]*a);

% magnitude plot
figure(1)
subplot(2,1,1)
plot(w,20*log10(abs(H)), ...
     [0 wpass],[delta delta],'r--', ...
     [0 wpass],[-delta -delta],'r--', ...
     [wstop pi],[10*log10(Us_top) 10*log10(Us_top)],'r--')
xlabel('w')
ylabel('mag H(w) in dB')
axis([0 pi -55 10]);

% phase plot
subplot(2,1,2)
plot(w,angle(H))
axis([0,pi,-pi,pi])
xlabel('w'), ylabel('phase H(w)')


function [sigma_draw, Ft_draw, F_draw, Fcomp_draw] = VARdrawpost(VAR)
% =======================================================================
% Draw from the posterior distribution of a VAR model
% =======================================================================
% [sigma_draw, Ft_draw] = VARdrawpost(VAR)
% -----------------------------------------------------------------------
% INPUT
%   - VAR: structure, result of VARmodel function
% -----------------------------------------------------------------------
% OUPUT
%   - sigma_draw: draw from the posterior of VCV matrix of VAR residuals
%   - Ft_draw: draw from the posterior of Ft of VAR
% -----------------------------------------------------------------------
% EXAMPLE
%   - See VARToolbox_Code.m in "../Primer/"
% =======================================================================
% VAR Toolbox 3.0
% Ambrogio Cesa-Bianchi
% ambrogiocesabianchi@gmail.com
% March 2012. Updated November 2020
% -----------------------------------------------------------------------

%% Get relevant parameters from VAR structure
%===============================================
nobs = VAR.nobs;
k    = [];
X    = VAR.X;
const= VAR.const;
nvar = VAR.nvar;
nlag = VAR.nlag;

%% OLS estimates
%============================================
Ft_hat = VAR.Ft;
sigma_hat = VAR.sigma;
inv_sigma_hat = inv(sigma_hat);

%% Draw the VCV matrix (sigma)
%============================================
inv_sigma_draw = wishrnd(inv_sigma_hat/nobs,nobs);
sigma_draw     = inv(inv_sigma_draw);

%% Draw the coeffiecient matrix (Ft)
%============================================
% Compute VCV of coefficient matrix
aux1 = (X'*X)\eye(size(X,2));
aux2 = kron(sigma_draw, aux1);
aux2 = (aux2 + aux2')/2; % Force symmetry (might fail bc of numerical error)
Fthat_vec = Ft_hat(:);
Ftdraw = mvnrnd(Fthat_vec,aux2);
Ft_draw = reshape(Ftdraw,k,nvar);
F_draw = Ft_draw';
Fcomp_draw = [F_draw(:,1+const:nvar*nlag+const); eye(nvar*(nlag-1)) zeros(nvar*(nlag-1),nvar)];


%  Figure 3.35      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%script to generate Fig. 3.35
clf;
t=0:.05:10;
k=1;
ki=0;
numCL=[k ki];
denCL=[1 3 2+k ki];
y1=step(numCL,denCL,t);
ki=1;
numCL=[k ki];
denCL=[1 3 2+k ki];
y2=step(numCL,denCL,t);
k=10;
ki=5;
numCL=[k ki];
denCL=[1 3 2+k ki];
y3=step(numCL,denCL,t);
axis([0 10 0 1.2]);
plot(t,y1,t,y2,t,y3);
grid;
xlabel('Time (sec)');
ylabel('y(t)');
title('Fig. 3.35 Transient responses');
axis('normal');
text(2,.25,'K=1, K_I=0');
text(3,0.75,'K=1, K_I=1');
text(1.5,1.1,'K=10, K_I=5');


function test04 ( dim_num, level_max )

%*****************************************************************************80
%
%% TEST04 sums the weights and compares them to 2^DIM_NUM.
%
%  Discussion:
%
%    This routine gets the sparse grid indices and determines the
%    corresponding sparse grid abscissas.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the level.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04:\n' );
  fprintf ( 1, '  Compute the weights of a Clenshaw Curtis sparse grid .\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  As a simple test, sum these weights.\n' );
  fprintf ( 1, '  They should sum to exactly 2^DIM_NUM.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEVEL_MAX = %d\n', level_max );
  fprintf ( 1, '  Spatial dimension DIM_NUM = %d\n', dim_num );
%
%  Determine the number of points.
%
  point_num = sparse_grid_cfn_size ( dim_num, level_max );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of unique points in the grid = %d\n', point_num );
%
%  Compute the weights and points.
%
  [ grid_weight, grid_point ] = sparse_grid_cc ( dim_num, level_max, point_num );
%
%  Sum the weights.
%
  weight_sum = sum ( grid_weight(1:point_num) );

  weight_sum_exact = 2.0^dim_num;

  weight_sum_error = abs ( weight_sum - weight_sum_exact );

  fprintf ( 1, '\n' ) ;
  fprintf ( 1, '    Weight sum     Exact sum    Difference\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %12e  %12e  %12e\n', ...
    weight_sum, weight_sum_exact, weight_sum_error );

  return
end


function prob_test089 ( )

%*****************************************************************************80
%
%% TEST089 tests HALF_NORMAL_MEAN, HALF_NORMAL_SAMPLE, HALF_NORMAL_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST089\n' );
  fprintf ( 1, '  For the Half Normal PDF:\n' );
  fprintf ( 1, '  HALF_NORMAL_MEAN computes the mean;\n' );
  fprintf ( 1, '  HALF_NORMAL_SAMPLE samples;\n' );
  fprintf ( 1, '  HALF_NORMAL_VARIANCE computes the variance.\n' );

  a = 0.0;
  b = 10.0;

  check = half_normal_check ( a, b );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST089 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  mean = half_normal_mean ( a, b );
  variance = half_normal_variance ( a, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A =             %14f\n', a );
  fprintf ( 1, '  PDF parameter B =             %14f\n', b );
  fprintf ( 1, '  PDF mean =                    %14f\n', mean );
  fprintf ( 1, '  PDF variance =                %14f\n', variance );

  for i = 1 : nsample
    [ x(i), seed ] = half_normal_sample ( a, b, seed );
  end

  mean = r8vec_mean ( nsample, x );
  variance = r8vec_variance ( nsample, x );
  xmax = max ( x(1:nsample) );
  xmin = min ( x(1:nsample) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =     %6d\n', nsample );
  fprintf ( 1, '  Sample mean =     %14f\n', mean );
  fprintf ( 1, '  Sample variance = %14f\n', variance );
  fprintf ( 1, '  Sample maximum =  %14f\n', xmax );
  fprintf ( 1, '  Sample minimum =  %14f\n', xmin );

  return
end


function geometry_test02015 ( )

%*****************************************************************************80
%
%% TEST02015 tests CYLINDER_POINT_DIST_SIGNED_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;
  test_num = 6;

  dist_test = [ 3.0, -0.5, 5.0, 8.0, -1.0, -0.25 ];
  p_test = [ ...
      4.0,   0.5,  0.0; ...
     -0.5,  -1.0,  0.0; ...
      4.0,   6.0,  0.0; ...
      0.75, -10.0, 0.0; ...
      0.0,   0.0,  0.0; ...
      0.25,  1.75, 0.0 ]';
  p1 = [ 0.0, -2.0, 0.0 ];
  p2 = [ 0.0,  2.0, 0.0 ];
  r = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02015\n' );
  fprintf ( 1, '  CYLINDER_POINT_DIST_SIGNED_3D computes the signed\n' );
  fprintf ( 1, '  distance to a cylinder.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Radius R = %f\n', r );
  fprintf ( 1, '  Center of bottom disk = %10f  %10f  %10f\n', p1(1:dim_num) );
  fprintf ( 1, '  Center of top disk =    %10f  %10f  %10f\n', p2(1:dim_num) );

  for test = 1 : test_num

    p(1:dim_num) = p_test(1:dim_num,test);

    fprintf ( 1, '\n' );
    fprintf ( 1, '  P =    %10f  %10f  %10f\n', p(1:dim_num) );

    dist = cylinder_point_dist_signed_3d ( p1, p2, r, p );

    fprintf ( 1, '  Signed distance (computed) = %f\n', dist );
    fprintf ( 1, '  Signed distance (exact) =    %f\n', dist_test(test) );

  end

  return
end


function [bw] = kernelbw(xi)
%% Silverman bandwidth

N      = length(xi);
prop   = 1.0;
sig    = std(xi);
iqrSig = 0.7413 * iqr(xi);

if (max(iqrSig) == 0)
   iqrSig = sig;
end

bw = prop * min(sig, iqrSig) * N^(-1 / (4 + 1));

end


%% DNLS Example1
clc
% ODE System
 ode = @(t,z,p) [-p(1)*z(1) + 4;
                 2*z(1) - p(1)*z(2) + 5;
                 -4*z(1) - 2*z(2)*z(3) - p(2)];

% Initial Conditions
 z0 = [-1.5;1.25;1];

% True Parameter Values
 p = [2.345;1.1];

% Generate Fitting Data
 tm  = 0:0.1:2;                 %measurement times
 odeInt = @(t,z) ode(t,z,p);    %ODE function for ODE45
[~,zm] = ode45(odeInt,tm,z0);  %Solve ODEs

% Starting Guess
 theta0 = [1;0.5];

% Create OPTI Object
 Opt = opti('ode',ode,'data',tm,zm,'z0',z0,'theta0',theta0)

% Solve
[theta,fval,exitflag,info] = solve(Opt)

% Plot the Solution
plot(Opt)

%% DNLS Example2
clc
% Indicate Initial Conditions to Solve for Using NaNs
 z0 = [-1.5;NaN;NaN];

% Starting Guess [p;z0]
 theta0 = [1;0.5;0.5;0.5];

% Create OPTI Object
 Opt = opti('ode',ode,'data',tm,zm,'z0',z0,'theta0',theta0)

% Solve
[theta,fval,exitflag,info] = solve(Opt)

% Plot the Solution
plot(Opt)

%% Example 3
clc

% Add Bounds
 lb = [1.5;0;0.1;0.1];
 ub = [3.5;2;1.5;1.5];

% Create OPTI Object
 Opt = opti('ode',ode,'data',tm,zm,'bounds',lb,ub,'z0',z0,'theta0',theta0)

% Solve
[theta,fval,exitflag,info] = solve(Opt)

% Plot the Solution
plot(Opt)

%% DNLS Example4
clc
% Set Dynamic Options (specifying states of interest)
 dopts = optidynset('stateIndex',[2 3]);

% Add to General OPTI Options
 opts = optiset('display','iter','dynamicOpts',dopts);

% Index Measured Data
 zm23 = zm(:,[2 3]);

% Create OPTI Object
 Opt = opti('ode',ode,'data',tm,zm23,'z0',z0,'theta0',theta0,'options',opts)

% Solve
[theta,fval,exitflag,info] = solve(Opt)

% Plot the Solution
plot(Opt)

%% DNLS Example5
clc
% Initial Conditions
z0 = [-1.5;1.25;1];

% Measurement Times for each State
tm2  = 0:0.1:2;                %state 2
tm3  = 0.2:0.2:2;              %state 3

% Solve ODEs and index Measurement Data
[~,zm2] = ode45(odeInt,tm2,z0); zm2 = zm2(:,2);
% Note 0 is required below just to match initial condition in this example
[~,zm3] = ode45(odeInt,[0 tm3],z0); zm3 = zm3(2:end,3); %drop first point

% Group measurements and time stamps in cell arrays
tm_multi = {tm2;tm3};
zm_multi = {zm2;zm3};

% Indicate Initial Conditions to Solve for Using NaNs
 z0 = [-1.5;NaN;NaN];

% Create OPTI Object
Opt = opti('ode',ode,'data',tm_multi,zm_multi,'z0',z0,'theta0',theta0,'options',opts)

% Solve
[theta,fval,exitflag,info] = solve(Opt)

% Plot the Solution
plot(Opt)

%% DNLS Example6
clc
% Initial Conditions
z0 = [-1.5;1.25;1];

% Measurement Times for each State
tm1  = 0.5:0.1:2;   %state 1
tm2  = 1:0.1:2;    %state 2
tm3  = 0.2:0.2:2;   %state 3

% Solve ODEs and index Measurement Data
% Note 0 is required below just to match initial condition in this example
[~,zm1] = ode45(odeInt,[0 tm1],z0); zm1 = zm1((2:end),1);
[~,zm2] = ode45(odeInt,[0 tm2],z0); zm2 = zm2((2:end),2);
[~,zm3] = ode45(odeInt,[0 tm3],z0); zm3 = zm3((2:end),3);

% Group measurements and time stamps in cell arrays
tm_multi = {tm1;tm2;tm3};
zm_multi = {zm1;zm2;zm3};

% Indicate Initial Conditions to Solve for Using NaNs
 z0 = [NaN;NaN;NaN];

% New Initial Guess Vector [p;z0];
 theta0 = [1;0.5;0.5;0.5;0.5];

% Create OPTI Object
dopts = optidynset('initialT',0);
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_multi,zm_multi,'z0',z0,'theta0',theta0,'options',opts)

% Solve
[theta,fval,exitflag,info] = solve(Opt)

% Plot the Solution
plot(Opt)

%% DNLS Example7
clc
% Initial Conditions
z0 = [-1.5;1.25;1];

% Measurement Times for each State
tm1  = 0.5:0.1:2;   %state 1
tm1b = 1:0.25:2;   %state 1
tm2  = 1:0.1:2;    %state 2
tm3  = 0.2:0.2:2;   %state 3

%ODE with modified parameters (representing a different run)
odeIntB = @(t,z) ode(t,z,p*0.85);

% Solve ODEs and index Measurement Data
% Note 0 is required below just to match initial condition in this example
[~,zm1]  = ode45(odeInt,[0 tm1],z0);  zm1  = zm1((2:end),1);
[~,zm1b] = ode45(odeIntB,[0 tm1b],z0); zm1b = zm1b((2:end),1);
[~,zm2]  = ode45(odeInt,[0 tm2],z0);  zm2  = zm2((2:end),2);
[~,zm3]  = ode45(odeInt,[0 tm3],z0);  zm3  = zm3((2:end),3);
[~,zm3b] = ode45(odeInt,[0 tm3],z0*0.75); zm3b = zm3b((2:end),3);

% Group measurements and time stamps in cell arrays
tm_multi = {[tm1 tm1b];tm2;[tm3 tm3]};
zm_multi = {[zm1;zm1b];zm2;[zm3;zm3b]};

% Indicate Initial Conditions to Solve for Using NaNs
 z0 = [NaN;NaN;NaN];

% New Initial Guess Vector [p;z0];
 theta0 = [1;0.5;0.5;0.5;0.5];

% Create OPTI Object
dopts = optidynset('initialT',0);
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_multi,zm_multi,'z0',z0,'theta0',theta0,'options',opts)

% Solve
[theta,fval,exitflag,info] = solve(Opt)

% Plot the Solution
plot(Opt)

%% DNLS Example8
clc
% Declare Symbolic Variables
syms p1 p2 z1 z2 z3
% Symbolic ODE
ODE = [-p1*z1 + 4;
      2*z1 - p1*z2 + 5;
      -4*z1 - 2*z2*z3 - p2];

% Evaluate Jacobians
dfdz_sym = jacobian(ODE,[z1 z2 z3])
dfdp_sym = jacobian(ODE,[p1 p2])

[dfdz,dfdp] = symDynJac(ode)

% Analytical Derivative Expressions
 dfdz = @(t,z,p) [-p(1) 0,       0;
                  2,    -p(1),   0;
                  -4,   -2*z(3), -2*z(2)];
 dfdp = @(t,z,p) [-z(1), 0;
                  -z(2), 0;
                  0,    -1];

% Set Dynamic Options
 dopts = optidynset('dfdz',dfdz,'dfdp',dfdp,'sensitivity','User','initialT',0);

% General OPTI Options
 opts = optiset('display','iter','derivCheck','on','dynamicOpts',dopts);

% Create OPTI Object
  Opt = opti('ode',ode,'data',tm_multi,zm_multi,'z0',z0,'theta0',theta0,...
             'options',opts)

% Solve
[theta,fval,exitflag,info] = solve(Opt)

% Plot the Solution
plot(Opt)

%% DNLS Ex9
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1)^2 - z(1)^3;
z0 = 0.001; %ic

%Generate measurement data
p = 1.5;                          %true parameter value
oi = @(t,z) ode(t,z,p);           %ode integrator function
tm = [0 65:70 80:1:90 100]*10;  %measurement times
[~,zm] = ode15s(oi,tm,z0);        %measurements

% Starting Guess
theta0 = 1;

%Build OPTI Object
dopts = optidynset('sensitivity','none');
opts1 = optiset('display','iter','iterfun',@optiplotlogfval,'dynamicOpts',dopts);
opts2 = optiset('display','iter','iterfun',@optiplotlogfval);

% Create OPTI Objects
OptNoDer = opti('ode',ode,'data',tm,zm,'z0',z0,'theta0',theta0,'options',opts1)
OptWDer = opti('ode',ode,'data',tm,zm,'z0',z0,'theta0',theta0,'options',opts2)

% Solve
[theta,fval,exitflag,info] = solve(OptNoDer)
[theta,fval,exitflag,info] = solve(OptWDer)

% Plot the Solution
subplot(121)
plot(OptNoDer); title('No Sensitivity');
subplot(122)
plot(OptWDer); title('With Sensitivity');

%% DNS Ex9b
clc
% Partial Derivatives
dfdz = @(t,z,p) 2*p(1)*z(1) - 3*z(1)^2;
dfdp = @(t,z,p) z(1)^2;

% Set Dynamic Options
dopts = optidynset('dfdz',dfdz,'dfdp',dfdp,'integrator','ode15s',...
                   'sensitivity','User');

% General OPTI Options
opts = optiset('display','iter','derivCheck','on','dynamicOpts',dopts);

% Create OPTI Object
Opt = opti('ode',ode,'data',tm,zm,'z0',z0,'theta0',theta0,'options',opts)

% Solve
[theta,fval,exitflag,info] = solve(Opt)

% Plot
plot(Opt)

%% DNS Ex9c
clc
% Set Dynamic Options
dopts = optidynset('integrator','ode15s','sensitivity','None');

% General OPTI Options
opts = optiset('solver','nomad','display','iter','dynamicOpts',dopts);

% Create OPTI Object
Opt = opti('ode',ode,'data',tm,zm,'bounds',0.5,2.5,'z0',z0,'theta0',theta0,'options',opts)

% Solve
[theta,fval,exitflag,info] = solve(Opt)


function M = perform_local_dct_transform(M,dir,w)

% perform_local_dct_transform - perform a JPEG-like transfrom
%
%   M = perform_local_dct_transform(M,dir,w, q);
%
%   dir=+1 : forward transform
%   dir=-1 : backward transform
%
%   w is the size of the blocks.
%
%   Copyright (c) 2004 Gabriel Peyre

if nargin<3
    w = 8;
end

n = size(M,1);

for i=1:floor(n/w);
    for j=1:floor(n/w);
        selx = (i-1)*w+1:min(i*w,n);
        sely = (j-1)*w+1:min(j*w,n);
        if dir==1
            M(selx,sely) = perform_dct_transform(M(selx,sely),+1);
        else
            M(selx,sely) = perform_dct_transform(M(selx,sely),-1);
        end
    end
end


function v = barywts(n)
%BARYWTS   Barycentric weights for Chebyshev points of 2nd kind.
%   BARYWTS(N) returns the N barycentric weights for polynomial interpolation on
%   a Chebyshev grid of the 2nd kind. The weights are normalised so that they
%   have infinity norm equal to 1 and the final entry is positive.
%
% See also BARY, CHEBPTS.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% See Thm. 5.2 of Trefethen, Approximation Theory and Approximation Practice,
% SIAM, 2013 for more information.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ( n == 0 )                      % Special case (no points)
    v = [];
elseif ( n == 1 )                  % Special case (single point)
    v = 1;
else                               % General case
    v = [ones(n-1,1) ; .5];        % Note v(end) is positive.
    v(end-1:-2:1) = -1;
    v(1) = .5*v(1);
end

end


function pass = test_feval( )
% Test spherefun feval.

tol = 1000*chebfunpref().cheb2Prefs.chebfun2eps;

% See the random number generator.
rng(9);

f = @(x,y,z) sin(x+ y.*z) + 1;
f = redefine_function_handle(f);
g = spherefun(f);
lambda = rand;
theta = rand;
pass(1) = abs( g(theta, lambda) - f(theta, lambda) ) < tol;

% feval at vectors:
lambda = rand(10, 1);
theta = rand(10, 1);
pass(2) = norm( g(theta, lambda) - f(theta, lambda), inf ) < tol;

% feval at row vectors:
lambda = rand(1, 10);
theta = rand(1, 10);
pass(3) = norm(g(theta, lambda) - f(theta, lambda) ) < tol;

% feval at vectors:
lambda = rand(2, 10);
theta = rand(2, 10);
pass(4) = norm(g(theta, lambda) - f(theta, lambda), inf ) < tol;

% feval at meshgrid:
[lambda, theta] = meshgrid(rand(3, 1));
pass(5) = norm(g(theta, lambda) - f(theta, lambda), inf) < tol;

% feval using Cartesian coordinates
f = @(x,y,z) sin(x+ y.*z) + 1;
x = rand(1, 3);
x = x/sqrt(sum(x.^2));
y = x(1, 2);
z = x(1, 3);
x = x(1, 1);
pass(6) = norm(g(x, y, z) - f(x, y, z), inf) < tol;

% feval using Cartesian coordinates at a vector of points
x = rand(10, 3);
nrm = sqrt(sum(x.^2,2));
y = x(:, 2)./nrm;
z = x(:, 3)./nrm;
x = x(:, 1)./nrm;
pass(7) = norm(g(x, y, z) - f(x, y, z), inf) < tol;

% feval using Cartesian coordinates at a vector of points
x = rand(10, 3);
y = rand(10, 3);
z = rand(10, 3);
nrm = sqrt(x.^2 + y.^2 + z.^2);
y = y./nrm;
z = z./nrm;
x = x./nrm;
pass(8) = norm(g(x, y, z) - f(x, y, z), inf) < tol;

% Check for the appropriate error flag.
try
    y = g(1,1,1);
    pass(9) = false;
catch ME
    pass(9) = strcmp(ME.identifier,'CHEBFUN:SPHEREFUN:FEVAL:pointsNotOnSphere');
end

end

function f = redefine_function_handle(f)
% nargin(f) = 2, then we are already on the sphere, if nargin(f) = 3,
% then do change of variables:

if ( nargin(f) == 3 )
    % Wrap f so it can be evaluated in spherical coordinates
    f = @(lam, th) spherefun.sphf2cartf(f, lam, th, 0);
end

end


% Driver script for solving the 2D Poisson equation on curvilinear domains
Globals2D;

% Polynomial order used for approximation
N = 6;

% Read in Mesh
[Nv, VX, VY, K, EToV] = MeshReaderGambitBC2D('circA01.neu');

% Initialize solver and construct grid and metric
StartUp2D;

% hard coded for all Dirichlet boundaries to be on cylinder
[k,f] = find(BCType==Dirichlet);
if(~isempty(k))
  cylfaces = [k,f];
  curved = sort(unique(k)); straight = setdiff(1:K, curved);
  MakeCylinder2D(cylfaces, 1, 0, 0);
end

% choose order to integrate exactly
Nint = ceil(2*N/2);

% build cubature nodes for all elements
CubatureOrder = 2*(Nint+1); cub = CubatureVolumeMesh2D(CubatureOrder);

% build Gauss node data for all element faces
NGauss = (Nint+1); gauss = GaussFaceMesh2D(NGauss);

% build weak Poisson operator matrices
[A, M] = CurvedPoissonIPDG2D();
Abc = CurvedPoissonIPDGbc2D();

% set up right hand side
f = (-2*pi^2-1)*sin(pi*x).*sin(pi*y);

% set up boundary condition
ubc = zeros(gauss.NGauss*Nfaces*K,1);
xbc = gauss.x(gauss.mapD); ybc = gauss.y(gauss.mapD);
ubc(gauss.mapD) = sin(pi*xbc).*sin(pi*ybc);
xbc = gauss.x(gauss.mapN); ybc = gauss.y(gauss.mapN);
ubc(gauss.mapN) = ...
    gauss.nx(gauss.mapN).*(pi*cos(pi*xbc).*sin(pi*ybc)) + ...
    gauss.ny(gauss.mapN).*(pi*sin(pi*xbc).*cos(pi*ybc));

ubc = Abc*ubc;

% solve linear system
solvec = (A+M)\(M*(-f(:)) + ubc);
u = reshape(solvec, Np, K);


function f = lnDiffCumGaussian(u, uprime)

% LNDIFFCUMGAUSSIAN Log of the difference between two cumulative Gaussians.
% FORMAT
% DESC computes the logarithm of the difference between two
% cumulative Gaussian distributions.
% ARG u1 : the argument of the first (positive) cumulative
% Gaussian.
% ARG u2 : the argument of the second (negative) cumulative
% Gaussian.
% RETURN f : where f = log(cumGaussian(u1) - cumGaussian(u2)).
%
% SEEALSO : cumGaussian, gaussOverDiffCumGaussian, lnCumGaussian
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006

% NDLUTIL

f = log(gaussOverDiffCumGaussian(u, uprime, 1)+1e-300) ...
    + .5*u.*u + .5*log(2*pi);
f=-f;


% Construction of a bilinearly blended Coons surface
%

%  D.M. Spink
%  Copyright (c) 2000.

% Boundary curve 1
pnts = [ 0.0  3.0  4.5  6.5 8.0 10.0;
         0.0  0.0  0.0  0.0 0.0  0.0;
         2.0  2.0  7.0  4.0 7.0  9.0];
crv1 = nrbmak(pnts, [0 0 0 1/3 0.5 2/3 1 1 1]);

% Boundary curve 2
pnts= [ 0.0  3.0  5.0  8.0 10.0;
        10.0 10.0 10.0 10.0 10.0;
        3.0  5.0  8.0  6.0 10.0];
crv2 = nrbmak(pnts, [0 0 0 1/3 2/3 1 1 1]);

% Boundary curve 3
pnts= [ 0.0 0.0 0.0 0.0;
        0.0 3.0 8.0 10.0;
        2.0 0.0 5.0 3.0];
crv3 = nrbmak(pnts, [0 0 0 0.5 1 1 1]);

% Boundary curve 4
pnts= [ 10.0 10.0 10.0 10.0 10.0;
        0.0   3.0  5.0  8.0 10.0;
        9.0   7.0  7.0 10.0 10.0];
crv4 = nrbmak(pnts, [0 0 0 0.25 0.75 1 1 1]);

srf = nrbcoons(crv1, crv2, crv3, crv4);

% Draw the surface
nrbplot(srf,[20 20]);
title('Construction of a bilinearly blended Coons surface.');


% Studies the recovery probability of rank aware order recursive
% matching pursuit with sparsity level and number of channels in joint recovery.

close all;
clear all;
clc;
rng('default');
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');
% Signal space
N = 256;
% Number of measurements
M = 32;
% Number of signals
Ss = [1, 2, 4, 8, 16, 32];
% Sparsity levels
Ks = 2:30;
num_trials = 200;
num_ks = length(Ks);
num_ss = length(Ss);
success_with_k = zeros(num_ss, num_ks);

snr_threshold = 100;

for ns=1:num_ss
    % Current sparsity level
    S = Ss(ns);
    for nk=1:num_ks
        K = Ks(nk);
        num_successes = 0;
        for nt=1:num_trials
            % Sensing matrix
            Phi = spx.dict.simple.gaussian_dict(M, N);
            % Sparse signal generator
            gen  = spx.data.synthetic.SparseSignalGenerator(N, K, S);
            % Gaussian distributed non-zero samples
            X = gen.gaussian;
            % Measurement vectors
            Y = Phi * X;
            % Create the solver for rank aware order recursive matching pursuit
            solver = spx.pursuit.joint.RankAwareORMP(Phi, K);
            result = solver.solve(Y);
            % Solution vectors
            X_Rec = result.Z;
            % Comparison
            cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
            % Reconstruction SNR
            snr = cs.cum_signal_to_noise_ratio;
            success = snr > snr_threshold;
            num_successes = num_successes + success;
            fprintf('S: %d, K=%d, trial=%d, residual: %e, SNR: %f dB\n'...
                , S, K, nt, cs.cum_difference_norm, snr);
        end
        success_with_k(ns, nk) = num_successes / num_trials;
    end
end

save ('bin/figure_1_ra_ormp_success_with_K_S.mat');


close all;
clearvars;
clc;
% The function
f=@(x,y) 2*x.^2 + 4*y.^2;;
% Construct a mesh of (x,y) values
[X,Y]=meshgrid(-3:0.1:3);
% compute f on all these (x, y) values
Z=f(X,Y);
% draw a contour plot
levels = [2:2:20];
[c, h] = contour3(X,Y,Z);
grid on;


function g = p26_g ( n, x )

%*****************************************************************************80
%
%% P26_G evaluates the gradient for problem 26.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real G(N), the gradient of the objective function.
%
  g = zeros ( n, 1 );

  jroot = 5;
  k = 500;
  fi = k;
  dfidx1 = 0.0;
  dfidx2 = 0.0;

  for j = 1 : jroot * jroot

    j1 = mod ( j-1, jroot ) + 1;
    a1 = -32 + j1 * 16;

    j2 = ( j - 1 ) / jroot;
    a2 = -32 + j2 * 16;

    fj = ( j + ( x(1) - a1 )^6 + ( x(2) - a2 )^6 );

    fi = fi + 1.0 / fj;

    dfidx1 = dfidx1 - ( 1.0 / fj^2 ) * 6.0 * ( x(1) - a1 )^5;
    dfidx2 = dfidx2 - ( 1.0 / fj^2 ) * 6.0 * ( x(2) - a2 )^5;

  end

  g(1) = - ( 1.0 / fi^2 ) * dfidx1;
  g(2) = - ( 1.0 / fi^2 ) * dfidx2;

  return
end


%% DEMOSLOPE  Short demonstration of slopes
%

%% Some sample applications of the Hessian toolbox
% Hessians implement second order automatic differentiation in forward mode, which is
% conveniently to implement using the Matlab operator concept.
%
% For a demonstration of the slope toolbox, intervals are wide; therefore, output
% is switched to infimum/supremum representation (see demointval):

setround(0)                           % set rounding to nearest
format compact short infsup

%% Slope expansions
% In order to use slope expansions, the expansion range and expansion
% point (interval) need to be identified and values have to be assigned.
% This is performed by the function "slopeinit", for example

u = slopeinit( [1;2] , infsup([1;2],[1.1;2.1]) )

%%
% The length (number of rows) is the number
% of independent variables, in the example above two. Slopes are always
% of type intval. For definition and use of slopes cf. Neumaier's book
% on interval analysis.

%% Computing with slopes
% Automatic slope expansion is performed by operations with slope variables.
% For example,

x = slopeinit(3.5,midrad(3.5,1e-4));
y = exp(3*x-sqrt(x))

%%
% evaluates the expression using slopes with expansion point 3.5 and expansion
% interval 3.5+/-1e-4.
%
% There is access to the center value, the range and
% the slope of y:

y.c
y.r
y.s

%%
% When evaluating the expression for another argument, e.g. a vector argument,
% use the same statement as before with new values.

xs = [2;3];
x = slopeinit(xs,midrad(xs,1e-4));
y = exp(3*x-sqrt(x))

%% Graphical illustration of slopes
% There is a simple graphic to demonstrate the behaviour of slope expansions for
% one-dimensional functions. The routine "slopeplot" assumes a function
% in one variable which can be evaluated for vectors, the expansion
% point and the expansion interval.
% For example,

format short
slopeplot('(x-2).*exp(x)-sqrt(abs(x))',1,infsup(1,2))

%%
% shows a best possible upper slope, but an underestimated lower slope.

%% Non-differentiable functions
% Functions to be handled by slopes need not be differentiable.
% An example is sqrt(abs(x)). Consider

slopeplot('sqrt(abs(x))',-2,infsup(-1,1))

%%
% The slope is best possible, but the grid is not fine enough to catch the
% extreme point at zero. For some 10000 grid points the picture looks
% as follows:

slopeplot('sqrt(abs(x))',-2,infsup(-1,1),[],10000)

%% Computation of the Gamma function with slopes
% Consider the following example in one unknown.
% According to Stirling's formula it is for u -> inf,
%
%                          1      1       139        571
%    Gamma(u) ~ C * ( 1 + --- + ----- - ------- - --------- + ... )
%                         12u       2         3           4
%                               288u    51840u    2488320u
% with
%
%         -u  u-0.5
%    C = e   u      sqrt(2*pi) .
%
% The following function evaluates Stirling's formula. It is also
% suited for vector input.
%
%
%   function y = g(u)
%       C = exp(-u) .* ( u.^(u-0.5) ) * sqrt(2.0*pi) ;
%       v = (((( -571.0/2488320.0 ./ u - 139.0/51840.0 ) ./ u ...
%                 + 1.0/288.0) ./ u ) + 1.0/12.0 ) ./ u + 1.0;
%       y = C .* v;
%
% A corresponding inline function is

format long e
g = @(u) ( ( exp(-u) .* ( u.^(u-0.5) ) * sqrt(2.0*pi) ) .* ...
           ( (((( -571.0/2488320.0 ./ u - 139.0/51840.0 ) ./ u ...
             + 1.0/288.0) ./ u ) + 1.0/12.0 ) ./ u + 1.0 ) )
u = [ 3.5 61 5 ]
g(u)

%% Computation of the range of the function g
% Next calculate the range of the function g within
% a certain interval. This can be performed, for example, by straightforward
% interval evluation (naive interval arithmetic)

format long
X = infsup(4.1,4.2);
Y = g(X)

%%
% or by slopes:

xs = 4.1;
X = infsup(4.1,4.2);
Ys = g(slopeinit(xs,X))

%%
% The range computed by the slope expansion is better by a factor 2 than
% naive interval arithmetic and less than 20 % overestimation of the true range:

diam(Y)
diam(Ys.r)
g(4.2)-g(4.1)

%%
% Note that Y or Ys.r is an inclusion of the range of g; for an inclusion
% of the range of the Gamma function an error
% term has to be added.

%% Slopes in several unknowns
% Automatic slope expansion with several unknowns works the same way.
% Consider the following example by Broyden.
%
%                                .5*sin(x1*x2) - x2/(4*pi) - x1/2  =  0
%  (1-1/(4*pi))*(exp(2*x1)-exp(1)) + exp(1)*x2/pi - 2*exp(1)*x1 )  =  0
%
% with initial approximation [ .6 ; 3 ] and one solution [ .5 ; pi ].
% The following function evaluates Broyden's function.
%
%  function  y = f1(x)
%     y = x;
%     y(1) = .5*sin(x(1)*x(2)) - x(2)/(4*pi) - x(1)/2;
%     y(2) = (1-1/(4*pi))*(exp(2*x(1))-exp(1)) + exp(1)*x(2)/pi - 2*exp(1)*x(1);
%
% The first statement is for efficiency. It is generally better to fix the size
% of an array before assigning values to the components. An inline function
% is as follows (cf. demogradient):

f = @(x) ( [ .5*sin(x(1)*x(2)) - x(2)/(4*pi) - x(1)/2 ; ...
             (1-1/(4*pi))*(exp(2*x(1))-exp(1)) + exp(1)*x(2)/pi - 2*exp(1)*x(1) ] )

%% Solution of a nonlinear system
% The nonlinear system defined by Broyden's function can solved by Newton's procedure
% as follows (cf. demogradient):

x = gradientinit([ .6 ; 3 ]);
for i=1:5
  y = f1(x);
  x = x - y.dx\y.x;
end
x

%%
% For simplicity, we omitted the stopping criterion.
% Here, y.dx is the Jacobian, y.x the function value at x.x, and -y.dx\y.x
% is the correction obtained by solution of a linear system.

%% Verified solution of a nonlinear system
% For verified solution of the nonlinear system, we need a correct definition
% of the function, see demogradient:
%
%  function  y = f(x)
%    y = x;
%    c1 = typeadj( 1 , typeof(x) );
%    cpi = typeadj( midrad(3.14159265358979323,1e-16) , typeof(x) );
%    y(1) = .5*sin(x(1)*x(2)) - x(2)/(4*cpi) - x(1)/2;
%    y(2) = (1-1/(4*cpi))*(exp(2*x(1))-exp(c1)) + exp(c1)*x(2)/cpi - 2*exp(c1)*x(1);
%
% This code is implemented in the function demotest.m .

%% Verified solution using verifynlss
% The nonlinear system defined by Broyden's function can be
% solved with verification and using slopes by:

y = verifynlss('demotest',[ .6 ; 3 ],'s')

%%
% The first parameter gives the name of the function, in this case "demotest",
% such that "demotest(x)" evaluates the function at "x".
% For the last parameter being 's', slopes are used, otherwise gradients (the default).
%
% We used slopes. However, the inclusion of the error with respect to an approximate
% is computed; therefore, the results for gradient and slope inclusion are identical.
% Note that use of gradients guarantees uniqueness of the zero within the computed
% interval, use of slope does not.

%% Nonlinear systems with uncertain parameters
% Next we artificially introduce an interval parameter of large diameter
% in Broyden's function:
%
%  function  y = demotest(x,radius)
%    y = x;
%    if nargin==1
%      radius = 1e-15;
%    end
%    cPi = typeadj( midrad(3.141592653589793,radius) , typeof(x) );
%    y(1) = .5*sin(x(1)*x(2)) - x(2)/(4*cPi) - x(1)/2;
%    y(2) = (1-1/(4*cPi))*(exp(2*x(1))-exp(1)) + exp(1)*x(2)/cPi - 2*exp(1)*x(1);
%
% This is to show that the range of applicability is larger for slopes than
% for gradients. For radius .04, both gradient expansion and slope expansion
% compute an inclusion (the extra parameter is passed to demotest):

radius = 4e-2; y1 = verifynlss('demotest',[ .6 ; 3 ],'g',[],radius)
radius = 4e-2; y2 = verifynlss('demotest',[ .6 ; 3 ],'s',[],radius)

%%
% The inclusion using slopes is better by 10 to 20 % in radius:

[ rad(y1) rad(y2) ]

%% A demo nonlinear function with large uncertainty
% Finally, we enlarge the radius of the parameter cPi to 0.05 and try to
% calculate an inclusion:

radius = 5e-2; y1 = verifynlss('demotest',[ .6 ; 3 ],'g',[],radius)
radius = 5e-2; y2 = verifynlss('demotest',[ .6 ; 3 ],'s',[],radius)

%%
% Now, only the slope expansion is able to compute an inclusion.

%% Enjoy INTLAB
% INTLAB was designed and written by S.M. Rump, head of the Institute for Reliable Computing,
% Hamburg University of Technology. Suggestions are always welcome to rump (at) tuhh.de


function Xd = Pendulum_Cart_Dynamics_Forced(X,F,P_Dyn)
% Xd = Pendulum_Cart_Dynamics_Forced(X,F,P_Dyn)
%
% FUNCTION:
%   This function is used to compute the dynamics of the pendulum cart
%   system. The inputs are the system state, actuator force, and paramters.
%
% INPUTS:
%
%   X = [4 x N] state matrix: [x;v;th;w]
%   F = [1 x N] actuator force (applied horizontally to cart)
%   P_Dyn = a parameter struct with fields:
%       M = cart mass
%       m = bob mass
%       L = pendulum length
%       g = gravitational acceleration
%
% OUTPUTS:
%   dX = [4 x N] derivative of the state matrix [dx;dv;dth;dw]
%
% NOTES:
%   The equations of motion are found using a cartesian force balance for
%   each rigid body
%
%   The cart is modelled as a point mass, and the pendulum is modelled as a
%   point mass at the end of a slender rod. There is no friction in the
%   system.
%
%   Parts of the derivation are shown below in comments. This code has been
%   partially optimized, so some of the comments show intermediate steps
%   that were bypassed.
%

%Define Parameters
m = P_Dyn.m;    %Pendulum point mass
M = P_Dyn.M;    %Cart mass
L = P_Dyn.L;    %Pendulum length (to point mass)
g = P_Dyn.g;    %Gravity

%Define input states:
% % x = X(1);    %Horizontal position of the cart
v = X(2,:);      %Horizontal velocity of the cart
th = X(3,:);     %Angle of the pendulum: 0 = straight up, pi = straight down
w = X(4,:);      %Angular rate of the pendulum

%Evaluate sines and cosines
Sin = -sin(th);
Cos = -cos(th);
One = ones(size(Sin));

%"Constant terms"
% % App_Forces = [...
% %     -F;
% %     M*g;
% %     -m*L*w^2*Sin;
% %     m*g + m*L*w^2*Cos];

% % %Matrix for of coeffieients:
% %     % Coeff * [Tension; Normal; Trans. Acc; Rot Acc] =
% %     % Applied_Forces( X0, Y0, X1, Y1 )
% % Coeff = [...
% %     sin(th)     0   -M  0;
% %     -cos(th)    1   0   0;
% %     -sin(th)    0   -m  -m*L*cos(th);
% %     cos(th)     0   0   -m*L*sin(th)];

% Analytic Inverse of Coeff via Mathematica:
Det = m*L*(M*Cos.^2 + (m+M)*Sin.^2);
inv_Det = 1./Det;
% % Adj = [...
% %     m^2*L*Sin       0   -m*M*L*Sin          m*M*L*Cos;
% %     m^2*L*Sin*Cos   1   -m*M*L*Sin*Cos      m*M*L*Cos^2;
% %     -m*L          	0   -m*L*Sin^2          m*L*Cos*Sin;
% %     m*Cos        	0   -M*Cos              -(M+m)*Sin];

% % Inv = inv_Det*Adj;
% % UnKnowns = Inv*App_Forces; %Solve linear system of equations

% % % % Tension = Unknowns(1);
% % % % Normal = UnKnowns(2);
% % vd = UnKnowns(3);
% % wd = UnKnowns(4);

%Unknowns (By Hand)
% % UnKnowns = inv_Det*[...
% %     %-F*               M*g*       -m*L*w^2*Sin*         (m*g + m*L*w^2*Cos)*
% %     -F*m^2*L*Sin     + 0   +  m*L*w^2*Sin*m*M*L*Sin     + (m*g + m*L*w^2*Cos)*m*M*L*Cos ;    %Tension
% %     -F*m^2*L*Sin*Cos + M*g +  m*L*w^2*Sin*m*M*L*Sin*Cos + (m*g + m*L*w^2*Cos)*m*M*L*Cos^2 ;  %Normal Force
% %     F*m*L            + 0   +  m*L*w^2*Sin*m*L*Sin^2     + (m*g + m*L*w^2*Cos)*m*L*Cos*Sin ;  %Translational acceleration
% %     -F*m*Cos         + 0   +  m*L*w^2*Sin*M*Cos         +  -(m*g + m*L*w^2*Cos)*(M+m)*Sin ]; %Angular Acceleration

%Broken Out UnKnowns (By Hand)
vd = (F*m*L  + 0 +  m*L*w.^2.*Sin*m*L.*Sin.^2 + (m*g*One + m*L*w.^2.*Cos).*m*L.*Cos.*Sin).*inv_Det;%Translational acceleration
wd = (-F.*m.*Cos  + 0  + m*L*w.^2.*Sin.*M.*Cos  +  -(m*g*One + m*L*w.^2.*Cos)*(M+m).*Sin).*inv_Det;%Angular Acceleration

%Defined derivatives:
xd = v;
thd = w;

%Express as a vector
Xd = [xd;vd;thd;wd];

end


function combo_test16 ( )

%*****************************************************************************80
%
%% COMBO_TEST16 tests MARRIAGE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
  n = 5;
%
%  PREFER(M,W) is the index of women W on man M's list.
%
  prefer = [ ...
    2, 1, 2, 1, 5; ...
    5, 2, 3, 3, 3; ...
    1, 3, 5, 2, 2; ...
    3, 4, 4, 4, 1; ...
    4, 5, 1, 5, 4 ]';
%
%  RANK(W,M) is the index of man M on woman W's list.
%
  rank = [ ...
    2, 4, 1, 4, 5; ...
    4, 3, 3, 2, 2; ...
    5, 5, 4, 1, 3; ...
    3, 1, 2, 3, 1; ...
    1, 2, 5, 5, 4 ]';

  fprintf ( 1, ' \n' );
  fprintf ( 1, 'COMBO_TEST16\n' );
  fprintf ( 1, '  MARRIAGE arranges a set of stable marriages\n' );
  fprintf ( 1, '  given a set of preferences.\n' );

  [ fiancee, next ] = marriage ( n, prefer, rank );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Man, Wife''s rank, Wife\n' );
  fprintf ( 1, ' \n' );
  for i = 1 : n
    fprintf ( 1, '  %6d  %6d  %6d\n', i, next(i), prefer(i,next(i)) );
  end

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Woman, Husband''s rank, Husband\n' );
  fprintf ( 1, ' \n' );
  for i = 1 : n
    fprintf ( 1, '  %6d  %6d  %6d\n', i, rank(i,fiancee(i)), fiancee(i) );
  end

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Correct result:\n' );
  fprintf ( 1, ' \n' );
  fprintf ( 1, '  M:W 1  2  3  4  5\n' );
  fprintf ( 1, '   1  +  .  .  .  .\n' );
  fprintf ( 1, '   2  .  .  .  +  .\n' );
  fprintf ( 1, '   3  .  .  .  .  +\n' );
  fprintf ( 1, '   4  .  .  +  .  .\n' );
  fprintf ( 1, '   5  .  +  .  .  .\n' );

  return
end


%  Figure 10.59      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% fig10_59.m is a script for disk read/write head control design
% the design is based on 1/s^2 with robustness
% with respect to a resonance of damping zeta = z at wm.
% the design calls for a single lead selected to maximize wc
% the compensator has a second order roll-off filter
% the resonance is 'gain stabilized' with GM
% the phase margin is set by alpha = a
% the time is scaled to milliseconds
wm= 5*pi
% input the nominal plant with a gain of 1.
numGo=1;
z=.05;
GM = 4;
a=0.1;
denGo= [1 0 0];
sysGo=tf(numGo,denGo);
% input the plant with the resonance
numG= [1/(50*pi) 1];
denG=[1/(25*pi^2) 1/(50*pi) 1  0 0];
sysG=tf(numG,denG);
% Define the design constant
A = ((a*2*z)/(GM))^.333;
% PM >50; we have selected alpha = a = 0.1
Tinv= A*wm*sqrt(a);
T=1/Tinv;
wc =  A*wm
% K =  wc^2;
K=  wc^2;
% Input the lead compensation
numD =K*sqrt(a)*[T 1];
denD = [a*T 1];
sysD=tf(numD,denD);
% Now design the roll-off filter
w1=wm*sqrt(A/sqrt(a));
% Design the roll-off damping
z1=.3 ;
numF=1;
denF=[1/w1^2  2*z1/w1 1];
sysF=tf(numF,denF);
% now compute the entire compensation
sysDc=sysD*sysF;
sysOLo=sysD*sysGo;
sysOL=sysDc*sysG;
clf;
% bode(sysOLo);
% hold on
w=logspace(-1,2);
[mag,ph]=bode(sysOL,w);
magdb=20*log10(mag(:,:));
magdb1=[magdb; zeros(size(magdb))];
subplot(211);
semilogx(w,magdb1);
grid on;
title('Fig. 10.59 Disk drive control with gain-stabilized resonance and roll-off filter');
xlabel('\omega (rad/sec)');
ylabel('Magnitude (db)');

subplot(212);
semilogx(w,[ph(:,:); -180*ones(size(ph(:,:)))]);
grid on;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
[Gm,Pm,Wcg,Wcp] = margin(mag,ph,w)


function [qout] = Filter_Slerp(trajNoisy,hlpf)
%This function will use SLERP  to lowpass filter a noisy trajectory which
%is represented as quaternions.

% The new filter state should be moved toward the current input  by a step
% size that is proportional to the distance between the current input and
% the current filter state. To do this, both the dist and slerp functions
% are used. The dist function returns a measurement in radians of the
% difference in rotation applied by two quaternions. The range of the dist
% function is the half-open interval [0,pi).

%Set parameters - The interpolation parameter to slerp is in the
%closed-interval [0,1] so the output of dist must be re-normalized to this
%range. However, the full range of [0,1] for the interpolation parameter
%gives poor performance so it is limited to a smaller range hrange centered
%at hbias.

% All options have been kept the same as the matlab example...
% Wrtten by Jayishni Maharaj (UQ)
% 15 Feb 2018

hrange = 0.7;%0.4
hbias = 0.7;%0.4
%Limit low and high to the interval [0, 1].
low  = max(min(hbias - (hrange./2), 1), 0);
high = max(min(hbias + (hrange./2), 1), 0);
hrangeLimited = high - low;

%Initialize the filter and preallocate outputs.
y = trajNoisy(1); % initial filter state
qout = zeros(size(y), 'like', y); % preallocate filter output
qout(1) = y;

for ii=2:numel(trajNoisy)
    x = trajNoisy(ii); clear d
    d = dist(y, x);

    % Renormalize dist output to the range [low, high]
    %hlpf = (d./pi).*hrangeLimited + low;
    y = slerp(y,x,hlpf);
    qout(ii,:) = y;
end
end


function pAzPts=polAzEquidistRefChange(pAzPts,latLonRefOld,latLonRefNew,a,f,algorithm,desAngSpan)
%%POLAZEQUIDISTREFCHANGE Points given in polar azimuthal equidistant
%   coordinates are defined with respect to a particular reference location
%   on the surface of the reference ellipsoid or sphere. Here, given one
%   set of polar azimuthal equidistant coordinates, obtain the coordinates
%   when the reference points has been changed. This can be useful for
%   expressing rays traced from one sensor in the coordinate system of
%   another sensor.
%
%INPUTS: pAzPts A 2XN set of the [ground distance; heading] points, with the
%              heading given in radians East of North, to convert.
%              Alternatively, if heights are also given, this can be a 3XN
%              set of points with the height being the third dimension.
% latLonRefOld A 2X1 [latitude;longitude] reference point in radians about
%              which pAzPts was computed.
% latLonRefNew A 2X1 [latitude;longitude] reference point in radians with
%              respect to which pAzPts should be converted.
%            a The semi-major axis of the reference ellipsoid (in meters).
%              If this argument is omitted or an empty matrix is passed,
%              the value in Constants.WGS84SemiMajorAxis is used.
%            f The flattening factor of the reference ellipsoid. If this
%              argument is omitted or an empty matrix is passed, the value
%              in Constants.WGS84Flattening is used.
%    algorithm If f is not zero, then this parameter is not used. If f=0,
%              then this parameter selects the algorithm to use. Possible
%              values are:
%              0 (The default if omitted or an empty matrix is used) Use a
%                formula specific for the transformation on a sphere.
%              1 Call the polarAzEquidistProj2Ellipse function and then the
%                ellips2PolarAzEquidistProj function.
%   desAngSpan A 2X1 range of angular values [min Val;max Val] in which it
%              is desired that the converted azimuthal values should
%              attempt to be kept (by adding or subtracing either 0 or 2*pi
%              [but not a multiple thereof] to the converted value to get
%              it as close as possible to the span). If omitted or an empty
%              matrix is passed, no attempt is made to keep the azimuth
%              within any particular bounds.
%
%OUTPUTS: pAzPts A 2XN (or 3XN) set of [ground distance;heading;(height)]
%                points in polar azimuthal equidistant coordinates taken
%                with respect to latLonRefNew.
%
%If f is 0 and algorithm=0, then the coordinate system change is occurring
%on a sphere and a direct conversion is used. Otherwise, this function just
%calls polarAzEquidistProj2Ellipse to convert the points into latitude and
%longitude coordinates and then uses ellips2PolarAzEquidistProj to convert
%them into polar azimuthal equidistant coordinates with a different
%reference point.
%
%EXAMPLE 1:
%This example shows that the relative error of this conversion is on the
%order of finite precision limitatios when compared to simply using the
%desired alternate reference point from the start.
% latLonPt=[deg2rad([20.631756;-155.551818;10]),deg2rad([21.295516;-158.002386;20])];
% latLonRef1=deg2rad([20.906029;-157.078918]);
% latLonRef2=deg2rad([35;-125]);
% pAzPts1=ellips2PolarAzEquidistProj(latLonPt,latLonRef1);
% pAzPts2=ellips2PolarAzEquidistProj(latLonPt,latLonRef2);
% pAzPtsConv=polAzEquidistRefChange(pAzPts1,latLonRef1,latLonRef2);
% RelErr=max(max(abs((pAzPtsConv-pAzPts2)./pAzPts2)))
%
%EXAMPLE 2:
%This example generates a number of random values and then computes the
%error in the conversion compared to directly converting it. The maximum
%relative error magnitude is displayed and is reasonable given the
%scale of the distances across the surface of the Earth and finite
%precision limits.
% rE=Constants.WGS84MeanRadius;
% numMCRuns=10e3;
% maxDiff=0;
% for curRun=1:numMCRuns
%     latLonRef1=[UniformD.rand(1,[-pi/2;pi/2]);
%                 UniformD.rand(1,[-pi;pi])];
%     latLonRef2=[UniformD.rand(1,[-pi/2;pi/2]);
%                 UniformD.rand(1,[-pi;pi])];
%     latLonPt=[UniformD.rand(1,[-pi/2;pi/2]);
%               UniformD.rand(1,[-pi;pi])];
%
%     pAzPts1=ellips2PolarAzEquidistProj(latLonPt,latLonRef1,rE,0);
%     pAzPts2=ellips2PolarAzEquidistProj(latLonPt,latLonRef2,rE,0);
%     pAzConv=polAzEquidistRefChange(pAzPts1,latLonRef1,latLonRef2,rE,0);
%
%     diffVal=(pAzPts2-pAzConv);
%     diffVal(2)=wrapRange(diffVal(2),-pi,pi);
%     diffVal=diffVal./pAzPts2;%Make a relative error.
%     maxDiff=max(maxDiff,norm(diffVal));
% end
% maxDiff
%
%December 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<7)
    desAngSpan=[];
end

if(nargin<6||isempty(algorithm))
    algorithm=0;
end

if(nargin<5||isempty(f))
    f=Constants.WGS84Flattening;
end

if(nargin<4||isempty(a))
    a=Constants.WGS84SemiMajorAxis;
end

if(algorithm~=0&&algorithm~=1)
    error('Unknown algorithm specified.')
end

if(f==0&&algorithm==0)
    [azStart,azEnd]=greatCircleAzimuth(latLonRefOld,latLonRefNew);
    azOffset1=pi/2-azStart;
    azOffset2=pi/2-azEnd;
    lambdaDelta=greatCircleDistance(latLonRefOld,latLonRefNew,1);

    %Transform into the system with both sensors on the equator.
    pAzPts(2,:)=pAzPts(2,:)+azOffset1;

    p=pAzPts(1,:);
    Az=pAzPts(2,:);
    rE=a;
    pAzPts(1:2,:)=[rE.*acos(cos(p./rE).*cos(lambdaDelta)+sin(p./rE).*sin(Az).*sin(lambdaDelta));
                   atan2(sin(p./rE).*cos(lambdaDelta).*sin(Az)-cos(p./rE).*sin(lambdaDelta),sin(p./rE).*cos(Az))];

    %Convert back to the global coordinate system.
    pAzPts(2,:)=pAzPts(2,:)-azOffset2;
else
    convLatLon=polarAzEquidistProj2Ellipse(pAzPts,latLonRefOld,a,f);
    pAzPts=ellips2PolarAzEquidistProj(convLatLon,latLonRefNew,a,f);
end

if(~isempty(desAngSpan))
    N=size(pAzPts,2);

    minVal=desAngSpan(1);
    maxVal=desAngSpan(2);

    for k=1:N
        curAz=pAzPts(2,k);

        if(minVal>curAz)
            %See if adding 2*pi places it closer to the span.
            azTest=curAz+2*pi;
            offsetOld=minVal-curAz;
        elseif(maxVal<curAz)
            %See if subtracting 2*pi places it closer to the span.
            azTest=curAz-2*pi;
            offsetOld=curAz-maxVal;
        else
            %It is in the span.
            continue;
        end

        %If it is closer to the span than before
        offsetNew=min(abs(minVal-azTest),abs(maxVal-azTest));

        if(offsetNew<offsetOld)
            pAzPts(2,k)=azTest;
        end
    end
end

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


function [ xdk, ddk ] = dif_derivk_table ( nd, xd, dd, k )

%*****************************************************************************80
%
%% DIF_DERIVK_TABLE computes the divided difference table for K-th derivative.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carl deBoor,
%    A Practical Guide to Splines,
%    Springer, 2001,
%    ISBN: 0387953663,
%    LC: QA1.A647.v27.
%
%  Parameters:
%
%    Input, integer ND, the size of the input table.
%
%    Input, real XD(ND), the abscissas for the divided
%    difference table.
%
%    Input, real DD(ND), the divided difference table.
%
%    Input, integer K, the index of the derivative.
%    0 <= K
%
%    Input, real XDK(ND-K), the abscissas for the divided
%    difference table for the derivative.
%
%    Output, real DDK(ND-K), the divided difference
%    table for the derivative.
%
  if ( k < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DIF_DERIVK_TABLE - Fatal error!\n' );
    fprintf ( 1, '  K < 0.\n' );
    error ( 'DIF_DERIVK_TABLE - Fatal error!' )
  end

  if ( nd <= k )
    ndk = 0;
    xdk = [];
    ydk = [];
    return
  end
%
%  Shift the abscissas to zero.
%
  ndk = nd;

  xd_temp(1:ndk) = xd(1:ndk);
  dd_temp(1:ndk) = dd(1:ndk);

  [ xd_temp, dd_temp ] = dif_shift_zero ( nd, xd_temp, dd_temp );
%
%  Repeatedly differentiate.
%
  for j = 1 : k

    ndk = ndk - 1;

    for i = 1 : ndk
      dd_temp(i) = i * dd_temp(i+1);
    end

  end

  ddk(1:ndk) = dd_temp(1:ndk);
  xdk(1:ndk) = 0.0;

  return
end


function lambda = rutis4_eigenvalues ( n )

%*****************************************************************************80
%
%% RUTIS4_EIGENVALUES returns the eigenvalues of the RUTIS4 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  for i = 1 : n
    angle = ( i ) * pi / ( 2 * ( n + 1 ) );
    lambda(i,1) = 64.0 * ( cos ( angle ) )^6;
  end

  return
end


function mask = coord2mask(M,N,vx,vy)
%COORD2MASK Generates a binary mask from given coordinates.
%   MASK = COORD2MASK(M,N,vx,yy) generates a binary mask, MASK, of size
%   M-by-N given a set of sequential coordinates [vx,yy] describing the
%   vertices of a polygon. The points inside and on the polygon are
%   labeled with 1s and all other points are labeled 0.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Create the mask by labeling as 1 all point inside or on the polygon
% defined by the vertices [vx,vy]. All other points in mask are labeled
% 0. This labeling is done automatically by MATLAB function poly2mask.
% NOTE: The syntax of this function is awkward. The order of vx (rows)
% and vy (columns) coordinates have to be reversed to match the book,
% but the M (number of rows) and N (number of columns) are input in the
% order used in the book.
mask = poly2mask(vy,vx,M,N);


function hermite_cubic_test13 ( )

%*****************************************************************************80
%
%% HERMITE_CUBIC_TEST13 tests HERMITE_CUBIC_LAGRANGE_INTEGRATE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HERMITE_CUBIC_TEST13:\n' );
  fprintf ( 1, '  HERMITE_CUBIC_LAGRANGE_INTEGRATE integrates a Hermite cubic\n' );
  fprintf ( 1, '  Lagrange polynomial from A to B.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compute each result TWICE:\n' );
  fprintf ( 1, '  First row computed using HERMITE_CUBIC_INTEGRATE.\n' );
  fprintf ( 1, '  Second row computed using HERMITE_CUBIC_LAGRANGE_INTEGRATE.\n' );

  x1 = 0.0;
  x2 = 10.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '        A           B           LF1         LD1         LF2         LD2\n' );
  fprintf ( 1, '\n' );

  a = x1 - 1.0;

  for j = - 3 : 12

    b = ( ( 10 - j ) * x1   ...
        +        j   * x2 ) ...
        /   10.0;

    p(1) = hermite_cubic_integrate ( x1, 1.0, 0.0, x2, 0.0, 0.0, a, b );
    p(2) = hermite_cubic_integrate ( x1, 0.0, 1.0, x2, 0.0, 0.0, a, b );
    p(3) = hermite_cubic_integrate ( x1, 0.0, 0.0, x2, 1.0, 0.0, a, b );
    p(4) = hermite_cubic_integrate ( x1, 0.0, 0.0, x2, 0.0, 1.0, a, b );

    q = hermite_cubic_lagrange_integrate ( x1, x2, a, b );

    fprintf ( 1, '  %10.4f  %10.4f  %10.4f  %10.4f  %10.4f  %10.4f\n', ...
      a, b, p(1:4) );
    fprintf ( 1, '                          %10.4f  %10.4f  %10.4f  %10.4f\n', ...
      q(1:4) );

  end

  return
end


function tform = geotrans(varargin)
%GEOTRANS Make affine and projective geometric transformations.
%   TFORM = GEOTRANS(TYPE,P1,P2,___) makes a geometric transformation
%   with the specified type and parameters.
%
%   Valid values for TYPE and P1, P2, ..., are:
%
%   'scale'     Scale. If only P1 is provided, it is the scale
%               factor in both directions. If P1 and P2 are provided,
%               then P1 is the horizontal scale factor and P2 is the
%               vertical scale factor.
%
%   'rotate'    Rotation. P1 is the rotation angle, measured in
%               degrees counterclockwise from the positive horizontal
%               axis.
%
%   'translate' Translation. P1 is the horizontal translation and P2
%               is the vertical translation.
%
%   'h-reflect' Horizontal reflection.
%
%   'v-reflect' Vertical reflection.
%
%   'h-shear'   Horizontal shear. P1 is the sheer angle in degrees,
%               measured counterclockwise from the downward-pointing
%               y-axis.
%
%   'v-shear'   Vertical shear. P1 is the sheer angle in degrees,
%               measured clockwise from the horizontal axis.
%
%
%   'compose'   Composition of transformations. P1, P2, ..., are
%               affine2d and projective2d transformations.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

if ischar(varargin{1}) || isstring(varargin{1})
   operation = varargin{1};
   switch operation
      case 'scale'
         narginchk(2,3);
         tform = scale(varargin{2:end});
      case 'rotate'
         narginchk(2,2);
         tform = rotate(varargin{2});
      case 'translate'
         narginchk(3,3);
         tform = translate(varargin{2:end});
      case 'h-reflect'
         narginchk(1,1);
         tform = reflectHorizontal;
      case 'v-reflect'
         narginchk(1,1);
         tform = reflectVertical;
      case 'h-shear'
         narginchk(2,2);
         tform = shearHorizontal(varargin{2});
      case 'v-shear'
         narginchk(2,2);
         tform = shearVertical(varargin{2});
      case 'compose'
         narginchk(2,Inf);
         tform = compose(varargin{2:end});
      otherwise
         error('Unrecognized operation: %s',operation);
   end
else
   error('Input argument TYPE must be a string or char vector.');
end

%----------------------------------------------------------------------%
function tform = scale(sx,sy)
if nargin < 2
   sy = sx;
end
tform = affine2d([sx 0 0; 0 sy 0; 0 0 1]);

%----------------------------------------------------------------------%
function tform = rotate(angle)
theta = (pi/180) * angle;
T = [ ...
   cos(theta)  sin(theta)  0
   -sin(theta) cos(theta)  0
   0           0           1 ];
tform = affine2d(T);

%----------------------------------------------------------------------%
function tform = translate(dx,dy)
T = [ ...
   1  0  0
   0  1  0
   dx dy 1 ];
tform = affine2d(T);

%----------------------------------------------------------------------%
function tform = reflectHorizontal
T = [ ...
   -1  0  0
   0  1  0
   0  0  1 ];
tform = affine2d(T);

%----------------------------------------------------------------------%
function tform = reflectVertical
T = [ ...
   1  0  0
   0  -1 0
   0  0  1];
tform = affine2d(T);

%----------------------------------------------------------------------%
function tform = shearVertical(angle_degrees)
alpha = tand(angle_degrees);
T = [ ...
   1 alpha 0
   0 1     0
   0 0     1 ];
tform = affine2d(T);

%----------------------------------------------------------------------%
function tform = shearHorizontal(angle_degrees)
alpha = tand(angle_degrees);
T = [ ...
   1     0  0
   alpha 1  0
   0     0  1];
tform = affine2d(T);

%----------------------------------------------------------------------%
function tform = compose(varargin)
all_affine = true;
for k = 1:nargin
   if ~(isa(varargin{k},'affine2d') || ...
         isa(varargin{k},'projective2d'))
      error('Input transforms must be affine2d or projective2d.');
   end

   if ~isa(varargin{k},'affine2d')
       all_affine = false;
   end

end
T = eye(3);
for k = 1:nargin
   tform_k = varargin{k};
   T = T * tform_k.T;
end
if all_affine
    tform = affine2d(T);
else
    tform = projective2d(T);
end


function i4mat_u1_inverse_test ( )

%*****************************************************************************80
%
%% I4MAT_U1_INVERSE_TEST tests I4MAT_U1_INVERSE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 6;
%
%  Each row of this definition is a COLUMN of the matrix.
%
  a = [ ...
    1,  0,  0,  0,  0,  0; ...
    2,  1,  0,  0,  0,  0; ...
    0,  0,  1,  0,  0,  0; ...
    5,  0,  3,  1,  0,  0; ...
    0,  0,  0,  0,  1,  0; ...
   75,  0,  0,  6,  4,  1 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4MAT_U1_INVERSE_TEST\n' );
  fprintf ( 1, '  I4MAT_U1_INVERSE inverts a unit upper triangular matrix.\n' );

  i4mat_print ( n, n, a, '  The original matrix:' );

  b = i4mat_u1_inverse ( n, a );

  i4mat_print ( n, n, b, '  The inverse matrix:' );

  c(1:n,1:n) = a(1:n,1:n) * b(1:n,1:n);

  i4mat_print ( n, n, c, '  The product:' );

  return
end


function [x, p, f]=numunique(x)
%NUMUNIQUE Numerical set unique.
% [B P F]=numunique(A) for the array A returns the same values as in A but
% with no repetitions. B will also be sorted. A can only be numerical,
% as the name of the function suggests.
%
% The second output, P, is a row of cells, containing the indices of A,
% such that, A(p{n})=B(n) is true. P has the same number of cells as the
% number of elements in B.
%
% Note that each cell of P lists all the indices of A, not only the first
% or last occurrence, which have the repetitious values. It is this nature
% of the second output makes this function different to the Mathwork's
% function UNIQUE. Sometimes we need to know all the indices of which A
% have the same values.
%
% The third optional output, F, is for the indices of the first occurrences
% of repetitious values, so that B==A(F) is true.
%
% Example, let  A = [1 1 5 6 2 3 3 9 8 6 2 4], then
%            [B P F]=numunique(A)
% results in
%            B =[1 2 3 4 5 6 8 9],
% P =
%  [1x2 double] [1x2 double] [1x2 double]  [12]  [3]  [1x2 double] [9] [8]
% and
% F =
%    1     5     6    12     3     4     9     8
%
% Check for the second element B for example, P{2}=[5 11],
% A([5 11])=[2 2], which are all equal to B(2). And of course, B-A(F)
% gives all 0's.
%
% Note that the first output can be a row or column vector depends on the
% input is a row vector or otherwise (column or matrix). However the second
% output P is always a row of cells. To force P in a row order has an
% advantage, so that you can get A sorted with a shortcut of A([P{:}]).
%
% See also UNIQUE
%
% Zhigang Xu. 02-Sep-2009. Updated on 05-Sep-2009.

isrowvec=size(x,1)==1;

if nargout<=1
	x=sort(x(:));
	n=[true; diff(x)~=0];
	x=x(n);
	if isrowvec, x=x.'; end
	return
end

[x s]=sort(x(:));
s=s.';
N=numel(x);
n=find([true; diff(x)~=0]);
x=x(n);
K=numel(x);
if isrowvec, x=x.'; end

switch(nargout)
	case{2}
		if K==1
			p{1}=s;
		else
			p{K}=s(n(K):N);
			for k=1:K-1
				p{k}=s(n(k):n(k+1)-1);
			end
		end
	case{3}
		if K==1
			p{1}=s;
			f(1)=p{1}(1);
		else
			p{K}=s(n(K):N);
			f(K)=p{K}(1);
			for k=1:K-1
				p{k}=s(n(k):n(k+1)-1);
				f(k)=p{k}(1);
			end
		end
	otherwise
		error('More than 3 outputs are not allowed!')
end
end


function error_frobenius = r8mat_is_eigen_left ( n, k, a, x, lambda )

%*****************************************************************************80
%
%% R8MAT_IS_EIGEN_LEFTT determines the error in a left eigensystem.
%
%  Discussion:
%
%    An R8MAT is a matrix of real values.
%
%    This routine computes the Frobenius norm of
%
%      X * A - LAMBDA * X
%
%    where
%
%      A is an N by N matrix,
%      X is an K by N matrix (each of K columns is an eigenvector)
%      LAMBDA is a K by K diagonal matrix of eigenvalues.
%
%    This routine assumes that A, X and LAMBDA are all real.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer K, the number of eigenvectors.
%    K is usually 1 or N.
%
%    Input, real A(N,N), the matrix.
%
%    Input, real X(K,N), the K eigenvectors.
%
%    Input, real LAMBDA(K), the K eigenvalues.
%
%    Output, real ERROR_FROBENIUS, the Frobenius norm
%    of X*A-LAMBDA*X.
%
  c = x * a;

  for i = 1 : k
    c(i,1:n) = c(i,1:n) - lambda(i) * x(i,1:n);
  end

  error_frobenius = r8mat_norm_fro ( k, n, c );

  return
end


%%
% Test for c-transform and Laguerre cells in 1D.

addpath('../toolbox/');

rep = 'results/c-transform-1d/';
[~,~] = mkdir(rep);

SetAR = @(ar)set(gca, 'PlotBoxAspectRatio', [1 ar 1]);

% # input points
N = 7;
% Grid size
q = 512;
rng('default'); rng(646);

M = rand(1,N);

% grid
t = linspace(0,1,q)';

% potentials
u = 0.1*randn(N,1);

% exponent/scale for the transform
if not(exist('p'))
    p = 2;
end
rho = 2;
if not(exist('epsilon'))
    epsilon = .01;
end

%%
% study influence of p

epsilon = 0;
p_list = linspace(.5,2,13);
lw = 2;
clf; hold on;
Col = [];
for ip=1:length(p_list)
    p = p_list(ip);
    s = (ip-1)/(length(p_list)-1);
    col = [0,s,1-s];  Col(end+1,:) = col;
    % v(x,y) = min_i c((x,y),m_i) - u_i
    D = abs( repmat(M(:), [1 q]) - repmat(t(:)', [N 1])  );
    S = rho * D.^p - repmat(u,[1 q]);
    %
    [v,I] = min(S);
    if epsilon>0
        S = S - repmat(v, [size(S,1) 1]);
        v = - epsilon * log( sum(  exp( -S/epsilon ) ) ) + v;
    end
    %
    plot(t, v, 'LineWidth', lw, 'Color', col);
end
%
plot(M, -u, 'r.', 'MarkerSize', 25);
axis tight; box on;
colormap(Col); caxis([p_list(1),p_list(end)]);
set(gca, 'FontSize', 20);
colorbar;
SetAR(1/2);
saveas(gcf, [rep 'c-transform-p.png'], 'png');
saveas(gcf, [rep 'c-transform-p.eps'], 'epsc');

%%
% study influence of epsilon

p = 1;
eps_list = linspace(0,.3,13);
lw = 2;
clf; hold on;
Col = [];
for ieps=1:length(eps_list)
    epsilon = eps_list(ieps);
    s = (ieps-1)/(length(eps_list)-1);
    col = [s,s,1-s];  Col(end+1,:) = col;
    % v(x,y) = min_i c((x,y),m_i) - u_i
    D = abs( repmat(M(:), [1 q]) - repmat(t(:)', [N 1])  );
    S = rho * D.^p - repmat(u,[1 q]);
    %
    [v,I] = min(S);
    if epsilon>0
        S = S - repmat(v, [size(S,1) 1]);
        v = - epsilon * log( sum(  exp( -S/epsilon ) ) ) + v;
    end
    %
    plot(t, v, 'LineWidth', lw, 'Color', col);
end
%
plot(M, -u, 'r.', 'MarkerSize', 25);
axis tight; box on;
colormap(Col); caxis([eps_list(1),eps_list(end)]);
set(gca, 'FontSize', 20);
colorbar;
SetAR(1/2);
saveas(gcf, [rep 'c-transform-eps.png'], 'png');
saveas(gcf, [rep 'c-transform-eps.eps'], 'epsc');


% HYPOTHESIS function.
% It predicts the output values y based on the input values X and model parameters.
function [predictions] = hypothesis(X, theta)
    % Input:
    % X - input features - (m x n) matrix.
    % theta - our model parameters - (n x 1) vector.
    %
    % Output:
    % predictions - output values that a calculated based on model parameters - (m x 1) vector.
    %
    % Where:
    % m - number of training examples,
    % n - number of features.

    predictions = sigmoid(X * theta);
end


function [u, flag] = minL1Tikhonov(f, gamma, A, opts)
%minL1Tikhonov Tikhonov regularization with L^1 data term
%
% Solves the optimization problem
%  \gamma \| u \|_2^2 + \| A u - f\|_1 -> min
% by a semi-smooth Newton method
%
% u = minL1Tikhonov(f, gamma, A)
%
%
% Reference:
%  Clason, Jin, Kunisch
%  A semismooth Newton method for L^1 data fitting with automatic choice of regularization parameters and noise calibration
%  SIAM Journal on Scientific Computing, 2010
%

% written by M. Storath
% $Date: 2012/10/29 01:19:08 $	$Revision: 0.1 $

%%-------------------------------------------------------------------------
% init
m = numel(f);
maxit = [];
init = [];
flag = 0;
if exist('opts', 'var')
    if isfield(opts, 'mat')
        AAT = opts.mat;
    end
    if isfield(opts, 'maxit')
        maxit = opts.maxit;
    end
    if isfield(opts, 'init')
        init = opts.init;
    end
    if isfield(opts, 'L')
        L = opts.L;
    end
end
if ~exist('AAT', 'var')
    AAT = A * A';
end
if ~isa(A, 'linop') && ~exist('L', 'var')
    L = spconvmatrix([-1, 2, -1], m);
end

% diagonal idx
diagidx = eye(m) == 1;

% standard parameters (see Clason, Jin, Kunisch 2010)
iMax = 20; %(iMax higher than standard)
c = 1e9;
beta = 1;
q = 0.2;

% init variables
p = zeros(m,1);
pos = p;
neg = p;
alpha = 2 * gamma;

%deactivate warnings temporaily
warning off;

%%-------------------------------------------------------------------------
% main program
while beta > 1e-16
    pOld = p;

    % precompute matrices
    if ~isa(A, 'linop')
        M = (1/alpha) * AAT + beta * L;
        diagM = M(diagidx);
    end

    % newton step
    for k = 1:iMax
        % compute active sets
        posNew = p > +1;
        negNew = p < -1;
        comb = posNew | negNew;

        % solve linear equation

        b = f(:) + c * (posNew - negNew);
        if isa(A, 'linop')
            M = @(x) reshape(AAT * (x / alpha), m, 1) + beta * conv(x(:), [-1; 2; -1], 'same') + c * comb .* x;
            [p, flag] = cgs(M, b, [], maxit, [], [], init);
        else
            M(diagidx) = diagM + c * comb;
            p = M \ b;
        end

        % if active sets did not change, break
        if ~(any(posNew - pos) || any(negNew - neg))
            break;
        end

        % set new active sets
        pos = posNew;
        neg = negNew;
    end

    if (k == iMax) && any(abs(p)>2)
        p = pOld;
        break;
    end

    % decrease beta
    beta = beta * q;
end

% restore primal variable
u = A' * (p / alpha);

% reactivate warnings
warning on;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: finds coefficients of cubic spline interpolant
%
% INPUTs: (p1,p2): interpolating mediary points
%
% Author: Nick Battista
% Date: February 23, 2018
% Institution: TCNJ
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function interp_Function_Coeffs(p1,p2)

%
% Set up MATRIX for linear system of cubic interpolant
%
mat1 = [1 0 0 0 0 0 0 0 0 0 0 0];
mat2 = [0 1 0 0 0 0 0 0 0 0 0 0];
mat3 = [0 0 1 0 0 0 0 0 0 0 0 0];
mat4 = [1 p1 p1^2 p1^3 -1 -p1 -p1^2 -p1^3 0 0 0 0];

mat5 = [0 1 2*p1 3*p1^2 0 -1 -2*p1 -3*p1^2 0 0 0 0];
mat6 = [0 0 2 6*p1 0 0 -2 -6*p1 0 0 0 0];
mat7 = [0 0 0 0 1 p2 p2^2 p2^3 -1 -p2 -p2^2 -p2^3];
mat8 = [0 0 0 0 0 1 2*p2 3*p2^2 0 -1 -2*p2 -3*p2^2];

mat9 = [0 0 0 0 0 0 2 6*p2 0 0 -2 -6*p2];
mat10= [0 0 0 0 0 0 0 0 1 1 1 1];
mat11= [0 0 0 0 0 0 0 0 0 1 2 3];
mat12= [0 0 0 0 0 0 0 0 0 0 2 6];

mat = [mat1; mat2; mat3; mat4; mat5; mat6; mat7; mat8; mat9; mat10; mat11; mat12];

%
% RHS of Linear System for cubic interpolant
%
rhs = [0 0 0 0 0 0 0 0 0 1 0 0]';

%
% Give coefficients
%
coeffs = mat\rhs

%------------------------------------
% Plot Interpolation Polynomial
%               (and its derivatives)
%------------------------------------

%Plotting t-Values
ds = 0.005;
t = 0:ds:1;
t2=t/2;

%Plot Attributes
ms = 20;

%
% PLOT: interpolating function, g(t)
figure(1)
for i=1:length(t)
    plot(t2(i),g(coeffs,t(i),p1,p2),'.','MarkerSize',ms); hold on;
end
xlabel('t');
ylabel('g(t)');
set(gca,'FontSize',18)

%
% PLOT: interpolating function's derivative, dg(t)/dt
figure(2)
for i=1:length(t)
    plot(t2(i),gP(coeffs,t(i),p1,p2),'.','MarkerSize',ms); hold on;
end
xlabel('t');
ylabel('dg/dt');
set(gca,'FontSize',18)

%
% PLOT: interpolating function's 2nd derivative, d^2g(t)/dt^2
figure(3)
for i=1:length(t)
    plot(t2(i),gPP(coeffs,t(i),p1,p2),'.','MarkerSize',ms); hold on;
end
xlabel('t');
ylabel('d^2g/dt^2');
set(gca,'FontSize',18)

%
% Store coefficients for plotting
%
% vec = [];
% for i=1:12
%     vec = [vec coeffs(i)];
% end

%strName = 'coeffs_Vec';
%print_Matrix_To_Txt_File(coeffs,strName);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION prints matrix to file
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Matrix_To_Txt_File(a,strName)

nameTxt = [strName '.txt'];

fid = fopen(nameTxt, 'wt'); % Open for writing
for i=1:size(a,1)
   fprintf(fid, '%.14f ', a(i,:));
   fprintf(fid, '\n');
end
fclose(fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: evaluate interpolating polynomial
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function val = g(coeffs,x,p1,p2)

if x<=p1
    val = coeffs(1) + coeffs(2)*x + coeffs(3)*x^2 + coeffs(4)*x^3;
elseif x<=p2
    val = coeffs(5) + coeffs(6)*x + coeffs(7)*x^2 + coeffs(8)*x^3;
else
    val = coeffs(9) + coeffs(10)*x + coeffs(11)*x^2 + coeffs(12)*x^3;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: evaluate interpolating polynomial 1st derivative
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function val = gP(coeffs,x,p1,p2)

if x<=p1
    val = coeffs(2) + 2*coeffs(3)*x + 3*coeffs(4)*x^2;
elseif x<=p2
    val = coeffs(6) + 2*coeffs(7)*x + 3*coeffs(8)*x^2;
else
    val = coeffs(10) + 2*coeffs(11)*x + 3*coeffs(12)*x^2;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: evaluate interpolating polynomial 2nd derivative
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function val = gPP(coeffs,x,p1,p2)

if x<=p1
    val = 2*coeffs(3) + 6*coeffs(4)*x;
elseif x<=p2
    val = 2*coeffs(7) + 6*coeffs(8)*x;
else
    val = 2*coeffs(11) + 6*coeffs(12)*x;
end


function X = SynthMMV(Y, HforVec, err, normfac)

% Solves problem of the form:
% min ||X||_2,1 s.t. ||Y-HX|| < err

% Inputs
% Y - n x r observation vectors
% H - n x N projection operator
% err - error tolerance
% normfac - maximum eigenvalue of projection operator

% Outputs
% X - N x r input vectors to be recovered

% Converting input matrix to operator
explicitA = ~(ischar(HforVec) || isa(HforVec, 'function_handle'));
if (explicitA)
    HOp = opMatrix(HforVec); clear HforVec
else
    HOp = HforVec;
end

r = size(Y,2); % number of observations/inputs

H = opMatWrapper(HOp, r); % wrapper for handling matrix inputs

alpha = 1.05*(normfac^2);

X = H(Y,2); % Initialize X
N = size(X,1); % length of each input vector

maxIter = 100; % Define the maximum number of iterations
tol = 1e-4; % tolerance level
decfac = 0.5; % decrease factor for lambda

lambdaInit = decfac*max(max((abs(X)))); lambda = lambdaInit;

while lambda > lambdaInit*tol
    iter = 0;
    while iter < maxIter
        iter = iter + 1;
        for i = 1:N
            D(i,:) = (1/norm(X(i,:))).*abs(X(i,:));
        end

        B = X + (1/alpha)*H(Y-H(X,1),2);

        Xvec = SoftTh(B(:),(lambda/alpha).*D(:));
        X = reshape(Xvec,[N r]);

    end
    if norm(Y-H(X,1),'fro') < err
        break;
    end
    lambda = decfac*lambda;
end

    function  z = SoftTh(s,thld)
        z = sign(s).*max(0,abs(s)-thld);
    end
end


function R = xyz2R(xyz)
%DESCRIPTION: given a point on earth retrun the rotation matrix with:
%           - fist lien directed to loacal east
%           - second line directed  as north
%           - third line directed as local up
[phi, lam] = cart2geod(xyz(1), xyz(2), xyz(3));

    %rotation matrix from global to local reference system
    R = [-sin(lam) cos(lam) 0;
         -sin(phi)*cos(lam) -sin(phi)*sin(lam) cos(phi);
         +cos(phi)*cos(lam) +cos(phi)*sin(lam) sin(phi)];
end


function [ax,ci,cen,ub,lb,S,e,lam,Fm,Fc,F,pval, msb] = conf_region(X,varargin)
% :Usage:
% ::
%
%    [ax,ci,cen,ub,lb,S,e,lam,Fm,Fc,F,pval, msb] = conf_region(X,[doplot])
%
% alternative conf region multivariate based on Johnson & Wichern, 4th ed., p. 236
% 2D (3D)
%
% :Outputs:
%
%   **ax:**
%        axes of confidence region, scaled to be half-length of confidence hyperellipsoid
%
%   **ci:**
%        length of axes of conf region (diagonal matrix), axes in reverse order of importance
%        as in ouput of eig
%
%   **cen:**
%        center of region (means of variables)
%
%   **ub:**
%        upper boundary coordinates for region, rows are dims (vars), cols index coordinates
%        last coordinate is on axis of greatest variation ("reverse order"), from output of eig
%
%   **lb:**
%        lower boundary coordinates
%        to plot, try: plot(ub(1,:),ub(2,:),'bo'); hold on; plot(lb(1,:),lb(2,:),'ro')
%
%   **S:**
%        covariance matrix
%
%   **e:**
%        eigenvectors
%
%   **lam:**
%        eigenvalues
%
%   **Fm:**
%        degrees of freedom multiplier, p(n-1) / n(n-p)
%
%   **Fc:**
%        critical F value based on alpha level
%
%   **F:**
%        F value for test - probably not quite right
%
%   **pval:**
%        p value for test - probably not quite right
%
% :To plot:
% Either enter 1 or color as a 2nd argument, or do it yourself:
% ::
%
%    [ax,ci,cen,ub,lb,S,e,lam] = conf_region(X);
%    b = pinv([X(:,1) ones(size(X,1),1)] ) * X(:,2);
%    theta = atan(b(1)) - pi/2;
%    [h,h2] = plot_ellipse(cen(1),cen(2),theta,ci(1,1),ci(2,2));
%
% There is also an example for rendering individual subject conf regions
%
% :Examples:
% ::
%
%    N = 250;
%    X = mvnrnd([1 2], [1 .6; .6 1], N);
%    [ax,ci,cen,ub,lb,S,e,lam] = conf_region(X);
%    b = pinv([X(:,1) ones(size(X,1),1)] ) * X(:,2);
%    theta = atan(b(1)) - pi/2;
%    %create_figure('test');
%    [h,h2] = plot_ellipse(cen(1),cen(2),theta,ci(1,1),ci(2,2));
%
%    % plot standard deviation - for boostrapping, or for individual cases
%    [h,h2] = plot_ellipse(cen(1),cen(2),theta,ci(1,1)*sqrt(N),ci(2,2)*sqrt(N));
%
% :Example: Render individual subject conf regions
% ::
%
%    X = x(wh, [3 1]);
%    dfe = size(X, 1) - size(X, 2);   % df
%    alph = .1;  % .1 for 90%, .05 for 95%
%    tcrit = tinv(1 - alph, dfe);
%    ci = ((lam) .^ .5) .* tcrit;
%    b = pinv([X(:,1) ones(size(X,1),1)] ) * X(:,2);
%    theta = atan(b(1)) - pi/2;
%    [h,h2] = plot_ellipse(cen(1),cen(2),theta,ci(1,1),ci(2,2));
%
%    set(h, 'Color', 'k', 'LineWidth', 1);
%    set(h2, 'FaceColor', [.5 1 0]);

doplot = 0;
if length(varargin) > 0, doplot = varargin{1}; end

% -----------------------------------------------------
% * determine multivariate standard deviation matrix S
% -----------------------------------------------------

% center
Xs = X - repmat(mean(X),size(X,1),1);

% covariance matrix S
S = (Xs' * Xs) ./ (size(Xs,1)-1);

% -----------------------------------------------------
% * get squared distance
% -----------------------------------------------------

% squared distance, Johnson & Wichern p. 201
% (X-mean(X))' * inv(S) * (X - mean(X)) generalized to matrices
d = Xs * inv(S) * Xs';
d = diag(d);            % what do the off-diagonals signify?

% -----------------------------------------------------
% * get critical F and coefficient of mult. for axes
% -----------------------------------------------------
p = size(X,2);
n = size(X,1);
Fm = (p * (n - 1)) ./ (n * (n - p));
a = .95;                                % alpha
Fc = finv(a,p,n - p);                   % critical F
coef = sqrt(Fm .* Fc);

[e,lam] = eig(S);

% -----------------------------------------------------
% * get  F and p-values
% ratio of squared distances (mahal?) between to within
% -----------------------------------------------------
msb = pdist([mean(X); zeros(1,size(X,2))]); % .^ 2;
msw = sum(d) ./ (n-p);
F = msb / msw;
pval = 1 - fcdf(F,p,n-p); % - this is wrong I think. do nonparam.

% nonparametric
niter = 1000;dnull = [];
signv = sign(randn(size(X,1),niter));
for i = 1:niter
    xtmp = X .* repmat(signv(:,i),1,size(X,2));
    dnull(i) = pdist([mean(xtmp); zeros(1,size(X,2))]);
end
pval = sum(msb <= dnull) ./ length(dnull);

% -----------------------------------------------------
% * confidence interval, in units - mult by axes.
% -----------------------------------------------------
ci = ((lam) .^ .5 .* coef);
ax = e * ci;

cen = mean(X)';
lb = repmat(cen,1,size(e,2)) - (e * ci);
ub = repmat(cen,1,size(e,2)) + (e * ci);

if doplot
    % 2-d plot

    % degrees or slope to angle in radians
    %ang = corrcoef(X); ang = ang(1,2);
    %theta = acos(ang-(pi/2));
    %pi * ang / 180

    b = pinv([X(:,1) ones(size(X,1),1)] ) * X(:,2);
    theta = atan(b(1)) - pi/2;
    [h,h2] = plot_ellipse(cen(1),cen(2),theta,ci(1,1),ci(2,2));
    set(h,'Color',[1 1 1],'LineWidth',.5);

    if length(doplot) == 3 , set(h2,'FaceColor',doplot); end
    if ischar(doplot), set(h2,'FaceColor',doplot(1)); end

    drawnow
end

return


function [s,c,r,t]=v_atan2sc(y,x)
%V_ATAN2SC    sin and cosine of atan(y/x) [S,C,R,T]=(Y,X)
%
% Outputs:
%    s    sin(t) where tan(t) = y/x
%    c    cos(t) where tan(t) = y/x
%    r    sqrt(x^2 + y^2)
%    t    arctan of y/x
%
% Note y and x can be arrays but must be the same size. The outputs will
% all be the same size as y.

%      Copyright (C) Mike Brookes 2007
%      Version: $Id: v_atan2sc.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

sz=size(y);
s=zeros(sz);
c=NaN(sz);
r=zeros(sz);
t=NaN(sz);
m=y==0;
if any(m(:)) % handle case when y=0 and possibly x=0 also
    t(m)=(x(m)<0);
    c(m)=1-2*t(m);
    r(m)=abs(x(m));
    t(m)=t(m)*pi;
end
m=abs(y)>abs(x) & isnan(c);
if any(m(:))
    q = x(m)./y(m);
    u = sqrt(1+q.^2).*sign(y(m)); % avoids underflow even if x and y are very small
    s(m) = 1./u;
    c(m) = s(m).*q;
    r(m) = y(m).*u;
end
m=isnan(c);
if any(m(:))
    q = y(m)./x(m);
    u = sqrt(1+q.^2).*sign(x(m));
    c(m) = 1./u;
    s(m) = c(m).*q;
    r(m) = x(m).*u;
end
m=isnan(t);
if nargout>3 && any(m(:))
    t(m)=atan2(s(m),c(m));
end


function [ add, mul, sub ] = setfld2 ( dummy )

%*****************************************************************************80
%
%% SETFLD2 sets up arithmetic tables for the finite field of order 2.
%
%  Discussion:
%
%    SETFLD2 sets up addition, multiplication, and subtraction tables
%    for the finite field of order 2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2003
%
%  Author:
%
%    Original FORTRAN77 version by Paul Bratley, Bennett Fox, Harald Niederreiter.
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer DUMMY, a dummy argument.
%
%    Output, integer ADD(2,2), MUL(2,2), SUB(2,2), the addition,
%    multiplication, and subtraction tables, mod 2.
%
  q = 2;

  p = 2;

  for i = 0 : 1
    for j = 0 : 1
      add(i+1,j+1) = mod ( i + j, p );
    end
  end

  for i = 0 : 1
    for j = 0 : 1
      mul(i+1,j+1) = mod ( i * j, p );
    end
  end

  for i = 0 : 1
    for j = 0 : 1
      sub(add(i+1,j+1)+1, i+1) = j;
    end
  end

  return
end