function varargout = harris(varargin)
% VL_HARRIS  Harris corner strength
%  H = VL_HARRIS(I,SI) computes the Harris corner strength of the image I
%  at ``integration'' scale SI.
%
%  The Harris strength [1] of a pixel is a statistic of the gradient
%  of the image integrated in a neighborhood of that pixel. This
%  neighborhood is a Gaussian window of variance SI.
%
%  In computing the Harris corner strength, there is a second scale
%  parameter, the ``derivation'' scale SD, which is the variance of
%  the Gaussian kernel used to pre-smooth the image I before computing
%  its gradient. SI and SD are independent parameters and VL_HARRIS(I,SI)
%  assumes that I is already smoothed at level SD.
%
%  VL_HARRIS(I,SI) uses Noble's variation [2] of the Harris score. If
%  SIGMAP and SIGMAM are respectively the biggest and smallest
%  eigenvalue of the structure tensor at a pixel, the score is given
%  by (SIGMAP*SIGMAM) / (SIGMAP+SIGMAM/2).  Let GAMMA = SIGMAM/SIGMAP
%  the ratio between the eigenvalues, which measures the degree of
%  anisotropy of the tensor and is always comprised in the range
%  [0,1].  Noble's score can be decomposed in two factors: the biggest
%  eigenvalue SIGMAP and the number
%
%    RHO = (2 GAMMA) / (GAMMA + 1).
%
%  RHO is another measure of isotropy that has value one for a
%  symmetric tensor and and zero for maximally anisotropic tensor.
%  [H,DETAILS] = VL_HARRIS(I,SIGMA) returns the additional structure
%  DETAILS with the following fields:
%
%    DETAILS.SIGMAP
%    DETAILS.RHO
%
%  VL_HARRIS(I,SI,ALPHA) uses Harris' original score [1], defined to be
%  SIGMAP*SIGMAM - ALPHA*(SIGMAP+SIGMAM)^2. This can be decomposed in
%  the factors SIGMAP^2 (note the square) and
%
%    RHO = GAMMA - ALPHA (1+GAMMA)^2.
%
%  Note that RHO is equal to -ALPHA for a maximally anisotropic
%  tensor. Typically ALPHA=0.04 and this is what is used by
%  VL_HARRIS(I,SI,[]).
%
%  REMARK. The gradient of the image I, used to compute the structure
%  tensor, is computed using central differencies. This means that a
%  function line [+1,-1,+1,...] has null Harris' score. This is
%  generally assumed to be a sampling artifact, and might be
%  avoided by oversampling the image.
%
%  EXAMPLE::
%    To extacts Harris points from image I:
%      idx = vl_localmax( vl_harris( vl_imsmooth( I, sd ), si ) ) ;
%      [i,j] = ind2sub( size(I), idx )
%
%  REFERENCES::
%    [1] C. Harris and M. Stephens, "A combined corner and edge detector,"
%    in Proceedings of The Fourth Alvey Vision Conference, pp. 147-151,
%    1988.
%
%    [2] J. A. Noble, "Finding corners, "Image Vision Computing, vol. 6,
%    no. 2, pp. 121-128, 1988.
%
%  See also: VL_HELP().
[varargout{1:nargout}] = vl_harris(varargin{:});


% -------------------------------------------------------------------------
% runduffsr.m
% Numerically solve the driven duffing oscillator with noise. Compute
% the averaged signal and noise amplitude spectra for varing noise
% strength. Plot signal-to-noise vs noise strength and time-series, phase
% space, well occupation distributions, and amplitude spectra for four
% chosen noise strengths.
% Dependencies: duff.m and noise.m
% -------------------------------------------------------------------------

clear all

tic % start timer

global p w

% -------------------------------------------------------------------------

X0 = [0 0]'; % initial conditions
t0 = 0; % initial time
tf = 1000; % final time
dt = 0.05; % time step
T = (t0:dt:tf); % time vector
options = [];

f = (1:length(T))/(length(T)*dt); % frequency
f = f(1:(ceil(length(T)/2))); % first half of frequency axis

win = hann(length(T)); % hanning window

wmax = 10; % noise cut-off frequency
alpha = 0.5; % strength of non-linear term
beta = 1; % strength of linear term
gam = 0.5; % damping strength
A = 0.38; % driving amplitude
w0 = 0.09; % driving frequency

N = 1000; % number of terms in noise sum

d = 0.05:0.05:2; % noise strength

% noise strength values for plots
dc = [0.1 0.5 1 2];

dw0 = w0*0.25; % integration range (w0+/- 25%)

M = 4; % number of terms in average (>= 2)

% initialize arrays
x = zeros(length(T),length(dc));
v = zeros(length(T),length(dc));
lc = zeros(1,length(dc));

Fd = zeros((1+length(T))/2,M);
Fdn = zeros((1+length(T))/2,M);

Sd = zeros((1+length(T))/2,length(d));
Sdn = zeros((1+length(T))/2,length(d));

Pd = zeros(1,length(d));
Pdn = zeros(1,length(d));

% -------------------------------------------------------------------------

for l = 1:length(d) % loop over noise strength array

n = d(l);

for j = 1:M % average amplitudes

p = unifrnd(0,2*pi,1,N); % random phases in noise
w = unifrnd(0,wmax,1,N); % random frequencies in noise

% numerically solve duffing oscillator
[t,y]=ode23(@duff,T,X0,options,gam,alpha,beta,A,w0,n);

% create x and v vectors at various noise strengths (d = 0.1,0.5,1.5,4)
% and get index for plots
if j == 1
    for i = 1:length(dc)
        if n == dc(i)
            x(:,i) = y(:,1);
            v(:,i) = y(:,2);
            lc(i) = l;
            break
        end
    end
end

% compute positive fft of windowed time-dependent signal and noise
% amplitudes
Fdtemp = fft(win.*y(:,1))/length(t); % signal fft
Fdntemp = fft(sqrt(n)*win.*noise(t)')/length(t); % noise fft

Fd(:,j) = Fdtemp(1:(ceil(length(t)/2))); % positive half of signal fft
Fdn(:,j) = Fdntemp(1:(ceil(length(t)/2))); % positive half of noise frequency

clear Fdtemp Fdntemp

end

% average frequency dependent signal and noise amplitude
Sd(:,l) = sum(abs(Fd'))/M; % averaged signal amplitude
Sdn(:,l) = sum(abs(Fdn'))/M; % averaged noise amplitude

i = 1;

% find signal and noise amplitude around driving frequency
for k = 1:length(f)
    if 2*pi*f(k) >= w0-dw0 && 2*pi*f(k) <= w0+dw0
        fr(i) = 2*pi*f(k); % frequency
        fd(i) = Sd(k,l); % signal amplitude
        fdn(i) = Sdn(k,l); % noise amplitude
        i = i + 1;
    end
end

% integrate signal and noise amplitude around driving frequency
Pd(l) = trapz(fr,fd); % signal "power"
Pdn(l) = trapz(fr,fdn); % noise "power"

clear fr fd fdn

end

% calculate and smooth snr (dB)
SNR = smooth(10*log(Pd./Pdn),0.1);

% -------------------------------------------------------------------------

figure(1) % plot snr vs noise strength
plot(d,SNR,'-ob')
xlabel('d'); ylabel('SNR (dB)');

% plots for various noise strengths (d = 0.1,0.5,1.5,4)
figure(2) % plot time-series
subplot(4,1,1)
plot(t,x(:,1))
xlabel('t'); ylabel('x');
subplot(4,1,2)
plot(t,x(:,2))
xlabel('t'); ylabel('x');
subplot(4,1,3)
plot(t,x(:,3))
xlabel('t'); ylabel('x');
subplot(4,1,4)
plot(t,x(:,4))
xlabel('t'); ylabel('x');

figure(3) % plot phase-space
subplot(2,2,1)
plot(x(:,1),v(:,1))
xlabel('x'); ylabel('v');
subplot(2,2,2)
plot(x(:,2),v(:,2))
xlabel('x'); ylabel('v');
subplot(2,2,3)
plot(x(:,3),v(:,3))
xlabel('x'); ylabel('v');
subplot(2,2,4)
plot(x(:,4),v(:,4))
xlabel('x'); ylabel('v');

figure(4) % plot histogram of well distribution
subplot(2,2,1)
hist(x(:,1),100)
xlabel('x');
subplot(2,2,2)
hist(x(:,2),100)
xlabel('x');
subplot(2,2,3)
hist(x(:,3),100)
xlabel('x');
subplot(2,2,4)
hist(x(:,4),100)
xlabel('x');

figure(5) % plot noise subtracted amplitude spectra
plot(2*pi*f,Sd(:,lc(1))-Sdn(:,lc(1)),2*pi*f,Sd(:,lc(2))-Sdn(:,lc(2)),...
     2*pi*f,Sd(:,lc(3))-Sdn(:,lc(3)),2*pi*f,Sd(:,lc(4))-Sdn(:,lc(4)))
legend('d = 0.1','d = 0.6','d = 1.8','d = 2.4')
xlabel('\omega'); ylabel('|A|');

toc % end timer


function Amatrix=createDFTAmatrix(X,H);
% function Amatrix=createDFTAmatrix(X,H);
% Creates matrix of DFT-amplitudes for a signal X.
% In each column is a spectrum (1:N/2+1), row indices are time-frame indices.
% H is the analysis window. N is the frame length, 50% overlap is used.

% Turn into column vector
X=X(:);
H=H(:);
Nx=length(X);
N=length(H);
M=floor(2*Nx/N-1);
Amatrix=zeros(N/2+1,M);
for k=1:M
    index=(k-1)*N/2+1:(k+1)*N/2;
    F=abs(fft(X(index).*H'));
    Amatrix(:,k)=F(1:N/2+1);
end


% Fig. W1 Web Appendix W8    Feedback Control of Dynamic Systems, 6e
%                                 Franklin, Powell, Emami
%

clear all;
close all;

F=[0 1;0 0];
G=[0;1];
H=[1 0];
J=0;
T=1;
[Phi,Gam]=c2d(F,G,T);
j=sqrt(-1);
Pc=[.78+.18*j;.78-.18*j];
K=acker(Phi,Gam,Pc);
Pe=[.2+.2*j;.2-.2*j];
L=acker(Phi',H',Pe)';
[A,B,C,D]=dreg(Phi,Gam,H,J,K,L);
A=Phi-Gam*K-L*H;
B=L;
C=K;
D=0;
[Ac,Bc,Cc,Dc]=series(A,B,C,D,Phi,Gam,H,J);
[Acl,Bcl,Ccl,Dcl]=feedback(Ac,Bc,Cc,Dc,0,0,0,1);
tf=30;
N=tf/T+1;
td=0:1:tf;
yd=dstep(Acl,Bcl,Ccl,Dcl,1,N);
axis([0 30 0 1.5])
plot(td,yd,'-',td,yd,'*'),
xlabel('time (sec)')
ylabel('plant output  y(t)')
title('Fig. 8.20  Step response of the continuous and digital systems')
nicegrid;
hold on

% use command structure from section 7.8

Nx=[1;0];
A2=[Phi  -Gam*K;
   L*H  Phi-L*H-Gam*K];
B2=[Gam*K*Nx;Gam*K*Nx];
C2=[1 0 0 0];
D2=0;
y2=dstep(A2,B2,C2,D2,1,N);
plot(td,y2,'-',td,y2,'o')
text(6.5,.25,'o-----o-----o-----o  Command structure from Fig 7.48(b)')
text(6.5,.45,'*-----*-----*-----*  Command structure from Fig 7.15')
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% GEOMETRIC ASIAN OPTION PRICER  (RUN SCRIPT)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Descritpion: Script to Price Geometric Asian options in Levy Models using the PROJ method
% Author:      Justin Kirkby
%
% Reference:   (1) An Efficient Transform Method For Asian Option Pricing, SIAM J. Financial Math., 2016
%              (2) Efficient Option Pricing by Frame Duality with the Fast Fourier Transform.
%                  SIAM J. Financial Math (2015), Kirkby, J.L
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[folder, name, ext] = fileparts(which( mfilename('fullpath')));
cd(folder);
addpath('../RN_CHF')
addpath('../Helper_Functions')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 1) CHOOSE CONTRACT/GENERAL PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
call = 1;    %For call use 1 (else, its a put)
S_0  = 100;  %Initial price
W    = 100;  %Strike            %NOTE: no error handling in place for extreme values of W (increase grid if strike falls outside)
r    = 0.05;  %Interest rate
q    = 0.00;  %dividend yield
T    = 1;    %Time (in years)
M    = 52;   %Number of monitoring points

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 2) CHOOSE MODEL PARAMETERS (Levy Models)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model = 1;   %See Models Below (e.g. model 1 is Black Scholes), and choose specific params
params = {};

if model == 1 %BSM (Black Scholes Merton)
    params.sigmaBSM = 0.15;    %CHOOSE

elseif model == 2 %CGMY
    params.C  = 0.02;
    params.G  = 5;
    params.MM = 15;
    params.Y  = 1.2;

elseif model == 3 %NIG
    params.alpha = 15;
    params.beta  = -5;
    params.delta = 0.5;

elseif model == 4 %MJD (Merton Jump Diffusion)
    params.sigma  = 0.12;
    params.lam    = 0.4;
    params.muj    = -0.12;
    params.sigmaj = 0.18;

elseif model == 5 %Kou Double Expo
    params.sigma = 0.15;
    params.lam   = 3;
    params.p_up  = 0.2;
    params.eta1  = 25;
    params.eta2  = 10;

elseif model == 8 % Variance Gamma
    params.sigma = 0.2;
    params.nu = 0.85;
    params.theta = 0;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 3) CHOOSE PROJ PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
UseCumulant = 1;  %Set to 1 to use the cumulant base rule (Approach 1) to determine gridwidth, else used fixed witdth (Approach 2)

%---------------------
% APPROACH 1: Cumulant Based approach for grid width
% (see "Robust Option Pricing with Characteritics Functions and the BSpline Order of Density Projection")
%---------------------
if UseCumulant ==1  %With cumulant based rule, choose N and Alpha (N = 2^(P+Pbar) based on second approach)
    logN  = 8;   %Uses N = 2^logN  gridpoint
    L1 = 10;  % determines grid witdth (usually set L1 = 8 to 15 for Levy)
%---------------------
% APPROACH 2: Manual GridWidth approach
%---------------------
else %Manually specify resolution and Pbar
    P     = 6;  % resolution is 2^P
    Pbar  = 3;  % Determines density truncation grid with, 2^Pbar
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% PRICE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Note: rnCHF is the risk netural CHF, c1,c2,c4 are the cumulants
dt = T/M;
modelInput = getModelInput(model, dt, r, q, params);

if UseCumulant ==1  % Choose density truncation width based on cumulants
    alpha = getTruncationAlpha(T, L1, modelInput, model);
else    % Manually supply density truncation width above
    logN = P + Pbar;
    alpha = 2^Pbar/2;
end
N = 2^logN;    % grid roughly centered on [c1 - alph, c1 + alph]

tic
price = PROJ_Geometric_Asian(N, alpha, S_0, M, W, call, T, r, q, modelInput.rnSYMB);
toc

fprintf('Geometric Price: %.8f \n', price)

compare_arithmetic = 1;

if compare_arithmetic
    addpath('../Asian_Options')
    price_arith = PROJ_Asian( N,alpha,S_0,M,W,call,T,r,q, modelInput.rnCHF, modelInput.RNmu*dt);
    fprintf('Arithmetic Price: %.8f \n', price_arith)
end


function [H] = rigidbody(f);

% RIGIDBODY creates the homogenous spatial transformation matrix
% for a 6 parameter rigid-body transformation
%
% Use as
%   [H] = rigidbody(f)
%
% The transformation vector f should contain the
%   x-shift
%   y-shift
%   z-shift
% followed by the
%   pitch (rotation around x-axis)
%   roll  (rotation around y-axis)
%   yaw   (rotation around z-axis)

% Copyright (C) 2000-2005, Robert Oostenveld
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% $Log: rigidbody.m,v $
% Revision 1.1  2009/01/30 04:02:11  arno
% *** empty log message ***
%
% Revision 1.4  2006/04/13 10:37:38  roboos
% added a ; to the end of a line
%
% Revision 1.3  2005/08/15 08:15:32  roboos
% reimplemented the rotate function, which contained an error (the error is in the AIR technical reference)
% changed all functions to be dependent on the rotate, translate and scale function
% all functions now behave consistenly, which also means that they are not compleetly backward compatible w.r.t. the order of the rotations
%
% Revision 1.2  2004/05/19 09:57:07  roberto
% added GPL copyright statement, added CVS log item
%

% compute the homogenous transformation matrix for the translation
T = translate(f([1 2 3]));

% compute the homogenous transformation matrix for the rotation
R = rotate_new(f([4 5 6]));

% compute the homogenous transformation matrix for the combination
H = T*R;


function [embedding_layer_state, hidden_layer_state, output_layer_state] = ...
  fprop(input_batch, word_embedding_weights, embed_to_hid_weights,...
  hid_to_output_weights, hid_bias, output_bias)
% This method forward propagates through a neural network.
% Inputs:
%   input_batch: The input data as a matrix of size numwords X batchsize where,
%     numwords is the number of words, batchsize is the number of data points.
%     So, if input_batch(i, j) = k then the ith word in data point j is word
%     index k of the vocabulary.
%
%   word_embedding_weights: Word embedding as a matrix of size
%     vocab_size X numhid1, where vocab_size is the size of the vocabulary
%     numhid1 is the dimensionality of the embedding space.
%
%   embed_to_hid_weights: Weights between the word embedding layer and hidden
%     layer as a matrix of size numhid1*numwords X numhid2, numhid2 is the
%     number of hidden units.
%
%   hid_to_output_weights: Weights between the hidden layer and output softmax
%               unit as a matrix of size numhid2 X vocab_size
%
%   hid_bias: Bias of the hidden layer as a matrix of size numhid2 X 1.
%
%   output_bias: Bias of the output layer as a matrix of size vocab_size X 1.
%
% Outputs:
%   embedding_layer_state: State of units in the embedding layer as a matrix of
%     size numhid1*numwords X batchsize
%
%   hidden_layer_state: State of units in the hidden layer as a matrix of size
%     numhid2 X batchsize
%
%   output_layer_state: State of units in the output layer as a matrix of size
%     vocab_size X batchsize
%

[numwords, batchsize] = size(input_batch);
[vocab_size, numhid1] = size(word_embedding_weights);
numhid2 = size(embed_to_hid_weights, 2);

%% COMPUTE STATE OF WORD EMBEDDING LAYER.
% Look up the inputs word indices in the word_embedding_weights matrix.
embedding_layer_state = reshape(...
  word_embedding_weights(reshape(input_batch, 1, []),:)',...
  numhid1 * numwords, []);

%% COMPUTE STATE OF HIDDEN LAYER.
% Compute inputs to hidden units.
inputs_to_hidden_units = embed_to_hid_weights' * embedding_layer_state + ...
  repmat(hid_bias, 1, batchsize);

% Apply logistic activation function.
% FILL IN CODE. Replace the line below by one of the options.
hidden_layer_state = zeros(numhid2, batchsize);
% Options
% (a) hidden_layer_state = 1 ./ (1 + exp(inputs_to_hidden_units));
% (b) hidden_layer_state = 1 ./ (1 - exp(-inputs_to_hidden_units));
hidden_layer_state = 1 ./ (1 + exp(-inputs_to_hidden_units));
% (d) hidden_layer_state = -1 ./ (1 + exp(-inputs_to_hidden_units));

%% COMPUTE STATE OF OUTPUT LAYER.
% Compute inputs to softmax.
% FILL IN CODE. Replace the line below by one of the options.
inputs_to_softmax = zeros(vocab_size, batchsize);
% Options
inputs_to_softmax = hid_to_output_weights' * hidden_layer_state +  repmat(output_bias, 1, batchsize);
% (b) inputs_to_softmax = hid_to_output_weights' * hidden_layer_state +  repmat(output_bias, batchsize, 1);
% (c) inputs_to_softmax = hidden_layer_state * hid_to_output_weights' +  repmat(output_bias, 1, batchsize);
% (d) inputs_to_softmax = hid_to_output_weights * hidden_layer_state +  repmat(output_bias, batchsize, 1);

% Subtract maximum.
% Remember that adding or subtracting the same constant from each input to a
% softmax unit does not affect the outputs. Here we are subtracting maximum to
% make all inputs <= 0. This prevents overflows when computing their
% exponents.
inputs_to_softmax = inputs_to_softmax...
  - repmat(max(inputs_to_softmax), vocab_size, 1);

% Compute exp.
output_layer_state = exp(inputs_to_softmax);

% Normalize to get probability distribution.
output_layer_state = output_layer_state ./ repmat(...
  sum(output_layer_state, 1), vocab_size, 1);


function checkdiff(problem, x, d)
% Checks the consistency of the cost function and directional derivatives.
%
% function checkdiff(problem)
% function checkdiff(problem, x)
% function checkdiff(problem, x, d)
%
% checkdiff performs a numerical test to check that the directional
% derivatives defined in the problem structure agree up to first order with
% the cost function at some point x, along some direction d. The test is
% based on a truncated Taylor series (see online Manopt documentation).
%
% Both x and d are optional and will be sampled at random if omitted.
%
% See also: checkgradient checkhessian

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%
%   April 3, 2015 (NB):
%       Works with the new StoreDB class system.

    % Verify that the problem description is sufficient.
    if ~canGetCost(problem)
        error('It seems no cost was provided.');
    end
    if ~canGetDirectionalDerivative(problem)
        error('It seems no directional derivatives were provided.');
    end

    x_isprovided = exist('x', 'var') && ~isempty(x);
    d_isprovided = exist('d', 'var') && ~isempty(d);

    if ~x_isprovided && d_isprovided
        error('If d is provided, x must be too, since d is tangent at x.');
    end

    % If x and / or d are not specified, pick them at random.
    if ~x_isprovided
        x = problem.M.rand();
    end
    if ~d_isprovided
        d = problem.M.randvec(x);
    end

    % Compute the value f0 at f and directional derivative at x along d.
    storedb = StoreDB();
    xkey = storedb.getNewKey();
    f0 = getCost(problem, x, storedb, xkey);
    df0 = getDirectionalDerivative(problem, x, d, storedb, xkey);

    % Compute the value of f at points on the geodesic (or approximation
    % of it) originating from x, along direction d, for stepsizes in a
    % large range given by h.
    h = logspace(-8, 0, 51);
    value = zeros(size(h));
    for i = 1 : length(h)
        y = problem.M.exp(x, d, h(i));
        ykey = storedb.getNewKey();
        value(i) = getCost(problem, y, storedb, ykey);
    end

    % Compute the linear approximation of the cost function using f0 and
    % df0 at the same points.
    model = polyval([df0 f0], h);

    % Compute the approximation error
    err = abs(model - value);

    % And plot it.
    loglog(h, err);
    title(sprintf(['Directional derivative check.\nThe slope of the '...
                   'continuous line should match that of the dashed '...
                   '(reference) line\nover at least a few orders of '...
                   'magnitude for h.']));
    xlabel('h');
    ylabel('Approximation error');

    line('xdata', [1e-8 1e0], 'ydata', [1e-8 1e8], ...
         'color', 'k', 'LineStyle', '--', ...
         'YLimInclude', 'off', 'XLimInclude', 'off');

    % In a numerically reasonable neighborhood, the error should decrease
    % as the square of the stepsize, i.e., in loglog scale, the error
    % should have a slope of 2.
    window_len = 10;
    [range, poly] = identify_linear_piece(log10(h), log10(err), window_len);
    hold on;
        loglog(h(range), 10.^polyval(poly, log10(h(range))), ...
               'r-', 'LineWidth', 3);
    hold off;

    fprintf('The slope should be 2. It appears to be: %g.\n', poly(1));
    fprintf(['If it is far from 2, then directional derivatives ' ...
             'might be erroneous.\n']);

end


function y = reflect(x,minx,maxx)

% function y = reflect(x,minx,maxx)
% Reflect the values in matrix x about the scalar values minx and maxx.
% Hence a vector x containing a long linearly increasing series
% is converted into a waveform which ramps linearly up and down
% between minx and maxx.
% If x contains integers and minx and maxx are (integers + 0.5),
% the ramps will have repeated max and min samples.
%
% Nick Kingsbury, Cambridge University, January 1999.

y = x;

% Reflect y in maxx.
t = find(y > maxx);
y(t) = 2*maxx - y(t);

% Reflect y in minx.
t = find(y < minx);
while ~isempty(t)
   y(t) = 2*minx - y(t);

   % Repeat until no more values out of range.
   t = find(y > maxx);
   if ~isempty(t), y(t) = 2*maxx - y(t); end
   t = find(y < minx);
end

return;


function [U, S, V, info] = truncated_svd(A, p)
% Returns an SVD decomposition of A truncated to rank p.
%
% function [U, S, V, info] = truncated_svd(A, p)
%
% Input: A real matrix A of size mxn and an integer p <= min(m, n).
% Output: An orthonormal matrix U of size mxp, an orthonormal matrix Y of
%         size nxp and a diagonal matrix S of size pxp with nonnegative and
%         decreasing diagonal entries such that USV.' is the best rank p
%         approximation of A according to the Frobenius norm. All real.
%         This function produces an output akin to svds.
%
% The decomposition is obtained by maximizing
%   f(U, V) = .5*norm(U'*A*V, 'fro')^2
% where U, V are orthonormal. Notice that f(U*Q, V*R) = f(U, V) for all
% Q, R orthogonal pxp matrices. Hence, only the column spaces of U and V
% matter and we may perform the optimization over a product of two
% Grassmannian manifolds.
%
% It is easy to show that maximizing f is equivalent to minimizing g with
%   g(U, V) = min_S norm(U*S*V' - A, 'fro')^2,
% which confirms that we are going for a best low-rank approximation of A.
%
% The inner workings of the Grassmann manifold use the built-in svd
% function of Matlab but only for matrices of size mxp and nxp to
% re-orthonormalize them.
%
% Notice that we are actually chasing a best fixed-rank approximation of a
% matrix, which is best obtained by working directly over a manifold of
% fixed-rank matrices. This is simply an example script to demonstrate some
% functionalities of the toolbox.
%
% The code can be modified to accept a function handle for A(x) = A*x
% instead of a matrix A, which is often useful. This would further require
% a function handle At for the transpose of A, such that At(x) = A.'*x.

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, July 5, 2013
% Contributors:
%
% Change log:
%
%   Aug. 23, 2021 (XJ):
%       Added AD to compute the egrad and the ehess

    % Generate some random data to test the function if none is given.
    if ~exist('A', 'var') || isempty(A)
        A = randn(42, 60);
    end
    if ~exist('p', 'var') || isempty(p)
        p = 5;
    end

    % Retrieve the size of the problem and make sure the requested
    % approximation rank is at most the maximum possible rank.
    [m, n] = size(A);
    assert(p <= min(m, n), 'p must be smaller than the smallest dimension of A.');

    % Define the cost and its derivatives on the Grassmann manifold
    tuple.U = grassmannfactory(m, p);
    tuple.V = grassmannfactory(n, p);
    % All of the code will work just as well if we ignore the invariance
    % property of the cost function indicated above and thus place U and V
    % on the Stiefel manifold (orthonormal matrices) instead of the
    % Grassmann manifold. Working on Stiefel is expected to be slower
    % though, partly because de search space is higher dimensional and
    % partly because the optimizers are not isolated.
    % tuple.U = stiefelfactory(m, p);
    % tuple.V = stiefelfactory(n, p);
    M = productmanifold(tuple);

    % Define a problem structure, specifying the manifold M, the cost
    % function and its derivatives. Here, to demonstrate the rapid
    % prototyping capabilities of Manopt, we directly define the Euclidean
    % gradient and the Euclidean Hessian egrad and ehess instead of the
    % Riemannian gradient and Hessian grad and hess. Manopt will take care
    % of the conversion. This automatic conversion is usually not
    % computationally optimal though, because much of the computations
    % involved in obtaining the gradient could be reused to obtain the
    % Hessian. After the prototyping stage, when efficiency becomes
    % important, it makes sense to define grad and hess rather than egrad
    % an ehess, and to use the caching system (the store structure).
    problem.M = M;
    problem.cost  = @cost;
    problem.egrad = @egrad;
    problem.ehess = @ehess;

    % The functions below make many redundant computations. This
    % performance hit can be alleviated by using the caching system.

    % Cost function
    function f = cost(X)
        U = X.U;
        V = X.V;
        f = -.5*norm(U'*A*V, 'fro')^2;
    end
    % Euclidean gradient of the cost function
    function g = egrad(X)
        U = X.U;
        V = X.V;
        AV = A*V;
        AtU = A'*U;
        g.U = -AV*(AV'*U);
        g.V = -AtU*(AtU'*V);
    end
    % Euclidean Hessian of the cost function
    function h = ehess(X, H)
        U = X.U;
        V = X.V;
        Udot = H.U;
        Vdot = H.V;
        AV = A*V;
        AtU = A'*U;
        AVdot = A*Vdot;
        AtUdot = A'*Udot;
        h.U = -(AVdot*AV'*U + AV*AVdot'*U + AV*AV'*Udot);
        h.V = -(AtUdot*AtU'*V + AtU*AtUdot'*V + AtU*AtU'*Vdot);
    end

    % An alternative way to compute the egrad and the ehess is to use
    % automatic differentiation provided in the deep learning toolbox
    % (slower). Notice that the function norm is not supported for AD so
    % far. Replace norm(...,'fro') with the backup function cnormsqfro
    % described in manoptADhelp
    % problem.cost = @cost_AD;
    %    function f = cost_AD(X)
    %        U = X.U;
    %        V = X.V;
    %        f = -.5*cnormsqfro(U'*A*V);
    %    end
    % call manoptAD to prepare AD for the problem structure
    % problem = manoptAD(problem);

    % Execute some checks on the derivatives for early debugging.
    % These things can be commented out of course.
    % checkgradient(problem);
    % pause;
    % checkhessian(problem);
    % pause;

    % Issue a call to a solver. A random initial guess will be chosen and
    % default options are selected. Here, we specify a maximum trust
    % region radius (which in turn induces an initial trust region radius).
    % Note that this is not required: default values are used if we omit
    % this. The diameter of the manifold scales like sqrt(2*p), hence the
    % form of our (empirical) choice.
    options.Delta_bar = 4*sqrt(2*p);
    [X, Xcost, info] = trustregions(problem, [], options); %#ok<ASGLU>
    U = X.U;
    V = X.V;

    % Finish the job by rotating U and V such that the middle matrix S can
    % be diagonal with nonnegative, decreasing entries. This requires a
    % small svd of size pxp.
    Spp = U'*A*V;
    [Upp, Spp, Vpp] = svd(Spp);
    U = U*Upp;
    S = Spp;
    V = V*Vpp;

    % For our information, Manopt can also compute the spectrum of the
    % Riemannian Hessian on the tangent space at (any) X. Computing the
    % spectrum at the solution gives us some idea of the conditioning of
    % the problem. If we were to implement a preconditioner for the
    % Hessian, this would also inform us on its performance.
    %
    % Notice that if the optimization is performed on a product of Stiefel
    % manifolds instead of a product of Grassmannians, the double
    % invariance under the orthogonal group O(p) will appear as twice
    % p*(p-1)/2, thus p*(p-1) zero eigenvalues in the spectrum of the
    % Hessian. This means that the minimizers are not isolated, which
    % typically hinders convergence of second order algorithms.
    if M.dim() < 512
        evs = hessianspectrum(problem, X);
        stairs(sort(evs));
        title(['Eigenvalues of the Hessian of the cost function ' ...
               'at the solution']);
    end

end


function cvx_optval = sum_smallest( x, varargin )

%SUM_SMALLEST Sum of the smallest k elements of a vector.
%   For a real vector X and an integer k between 1 and length(X) inclusive,
%   y = SUM_SMALLEST(X,k) is the sum of the k smallest elements of X; e.g.,
%       temp = sort( x )
%       y = sum( temp( 1 : k ) )
%   If k=1, then SUM_SMALLEST(X,k) is equivalent to MIN(X); if k=length(X),
%   then SUM_SMALLEST(X,k) is equivalent to SUM(X).
%
%   Both X and k must be real, and k must be a scalar. But k is not, in
%   fact, constrained to be an integer between 1 and length(X); the
%   function is extended continuously and logically to all real k. For
%   example, if k <= 0, then SUM_SMALLEST(X,k)=0. If k > length(X), then
%   SUM_SMALLEST(X,k)=SUM(X). Non-integer values of k interpolate linearly
%   between their integral neighbors.
%
%   For matrices, SUM_SMALLEST(X,k) is a row vector containing the
%   application of SUM_SMALLEST to each column. For N-D arrays, the
%   SUM_SMALLEST operation is applied to the first non-singleton dimension
%   of X.
%
%   SUM_SMALLEST(X,k,DIM) performs the operation along dimension DIM of X.
%
%   Disciplined convex programming information:
%       SUM_SMALLEST(X,...) is concave and nondecreasing in X. Thus, when
%       used in CVX expressions, X must be concave (or affine). k and DIM
%       must both be constant.

narginchk(2,3);
cvx_optval = -sum_largest( -x, varargin{:} );

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


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

clf;
num=1;
a=5;

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

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

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

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


function fx = p11_fun ( n, x )

%*****************************************************************************80
%
%% P11_FUN evaluates the integrand for problem 11.
%
%  Discussion:
%
%    S&S gives exact value as pi =  3.1415926535897932385...
%    S&S gives Laguerre(16) as      2.6652685196...
%    S&S gives EXP_TRANSFORM(16) as 2.3629036166...
%
%  Integral:
%
%    Integral ( 0 <= x < +oo ) 1/((1+x)*sqrt(x)) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the function values.
%
  for i = 1 : n
    if ( x(i) == 0.0 )
      fx(i) = 0.0;
    else
      fx(i) = 1.0 / ( ( 1.0 + x(i) ) * sqrt ( x(i) ) );
    end
  end

  return
end


function [RCM1, RCM2, RCM3, RCM4, RCM5] = region_covariance(img, options)
% Region covariance matrix (GCM) algorithm
%
% Inputs:
%       img                 image data
%       options             options
% Output:
%       RCM1                region covariance matrix of size 7x7 (RCM1)
%                           pixel location, intensity, 1st&2nd-order gradient
%       RCM2                region covariance matrix of size 7x7 (RCM2)
%                           pixel location, 1st&2nd-order gradient, and edge orientation
%       RCM3                region covariance matrix of size 6x6 (RCM3)
%                           pixel location, and 1st&2nd-order gradient
%       RCM4                region covariance matrix of size 8x8 (RCM4) with
%                           pixel location, intensity, 1st&2nd-order gradient, and
%                           edge orientation
%       RCM5                region covariance matrix of size 5x5 (RCM5) with
%                           intensity, 1st&2nd-order gradient (not appear in the paper)
%
%
% References:
%       Yanwei Pang, Yuan Yuan, and Xuelong Li
%       'Gabor-Based Region Covariance Matrices for Face Recognition'
%       IEEE Transactions on Circuits and Systems for Video Technology vol.18, no.7, 2008.
%
%
% Created by H.Kasai on June 23, 2017

    symm = @(X) .5*(X+X');

    % check correct number of arguments
    if (nargin ~= 2)
        error('Please use the correct number of input arguments!')
    end

    % extract options
    if ~isfield(options, 'spd_projection')
        spd_projection = true;
    else
        spd_projection = options.spd_projection;
    end

    % check if the input image is grayscale
    if size(img,3) == 3
        warning('The input RGB image is converted to grayscale!')
        img = rgb2gray(img);
    end

    % obtain image size
    [ysize, xsize] = size(img);
    num_of_pixels = ysize * xsize;

    % prepare array
    Feature_Mat = zeros(8, num_of_pixels);

    % calculate first-order and second-order derivatives
    img = double(img);
    [Ix, Iy] = gradient(img);   % first order partials
    [Ixx, Ixy] = gradient(Ix);  % second order partials
    [Iyx, Iyy] = gradient(Iy);  % second order partials

    % calculate 9-dimensional feature vectior for every pixels
    pixel_cnt = 0;
    for y = 1 : ysize
        for x = 1 : xsize
            %fprintf('[%d %d] %d)\n', y, x, pixel_cnt);
            pixel_cnt = pixel_cnt + 1;

            Feature_Mat(1, pixel_cnt)   = x;
            Feature_Mat(2, pixel_cnt)   = y;
            Feature_Mat(3, pixel_cnt)   = img(y,x);

            Feature_Mat(4, pixel_cnt)   = abs(Ix(y,x));
            Feature_Mat(5, pixel_cnt)   = abs(Iy(y,x));

            Feature_Mat(6, pixel_cnt)   = abs(Ixx(y,x));
            Feature_Mat(7, pixel_cnt)   = abs(Iyy(y,x));

            theta = atan(abs(Iy(y,x))/abs(Ix(y,x)));
            Feature_Mat(8, pixel_cnt)   = theta;

        end
    end

    % calculate covariance matrix
    Feature_Mat_type1 = Feature_Mat;
    Feature_Mat_type1(8,:) = [];            % remove orientation (theta)
    RCM1 = cov(Feature_Mat_type1');         % case for Eq.(5)

    Feature_Mat_type2 = Feature_Mat;
    Feature_Mat_type2(3,:) = [];            % remove intensity
    RCM2 = cov(Feature_Mat_type2');         % case for Eq.(6)

    Feature_Mat_type3 = Feature_Mat;
    Feature_Mat_type3(8,:) = [];            % remove edge orientation (theta)
    Feature_Mat_type3(3,:) = [];            % remove intensity
    RCM3 = cov(Feature_Mat_type3');         % case for Eq.(8)

    RCM4 = cov(Feature_Mat');               % case for Eq.(9)

    Feature_Mat_type5 = Feature_Mat;
    Feature_Mat_type5(8,:) = [];            % remove edge orientation (theta)
    Feature_Mat_type5(1:2,:) = [];          % remove location
    RCM5 = cov(Feature_Mat_type5');         % additional case

    % project onto SPD if needed
    if spd_projection
        RCM1 = symm(spd_project(RCM1));
        RCM2 = symm(spd_project(RCM2));
        RCM3 = symm(spd_project(RCM3));
        RCM4 = symm(spd_project(RCM4));
        RCM5 = symm(spd_project(RCM5));
    end

end


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


%% 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)')


%  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');


%% 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 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


% 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.');


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


%  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 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 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 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


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;