function [qrs_amp_raw,qrs_i_raw,delay]=pan_tompkin(ecg,fs,gr)

%% function [qrs_amp_raw,qrs_i_raw,delay]=pan_tompkin(ecg,fs)
% Complete implementation of Pan-Tompkins algorithm

%% Inputs
% ecg : raw ecg vector signal 1d signal
% fs : sampling frequency e.g. 200Hz, 400Hz and etc
% gr : flag to plot or not plot (set it 1 to have a plot or set it zero not
% to see any plots
%% Outputs
% qrs_amp_raw : amplitude of R waves amplitudes
% qrs_i_raw : index of R waves
% delay : number of samples which the signal is delayed due to the
% filtering
%% Method :

%% PreProcessing
% 1) Signal is preprocessed , if the sampling frequency is higher then it is downsampled
% and if it is lower upsampled to make the sampling frequency 200 Hz
% with the same filtering setups introduced in Pan
% tompkins paper (a combination of low pass and high pass filter 5-15 Hz)
% to get rid of the baseline wander and muscle noise.

% 2) The filtered signal
% is derivated using a derivating filter to high light the QRS complex.

% 3) Signal is squared.4)Signal is averaged with a moving window to get rid
% of noise (0.150 seconds length).

% 5) depending on the sampling frequency of your signal the filtering
% options are changed to best match the characteristics of your ecg signal

% 6) Unlike the other implementations in this implementation the desicion
% rule of the Pan tompkins is implemented completely.

%% Decision Rule
% At this point in the algorithm, the preceding stages have produced a roughly pulse-shaped
% waveform at the output of the MWI . The determination as to whether this pulse
% corresponds to a QRS complex (as opposed to a high-sloped T-wave or a noise artefact) is
% performed with an adaptive thresholding operation and other decision
% rules outlined below;

% a) FIDUCIAL MARK - The waveform is first processed to produce a set of weighted unit
% samples at the location of the MWI maxima. This is done in order to localize the QRS
% complex to a single instant of time. The w[k] weighting is the maxima value.

% b) THRESHOLDING - When analyzing the amplitude of the MWI output, the algorithm uses
% two threshold values (THR_SIG and THR_NOISE, appropriately initialized during a brief
% 2 second training phase) that continuously adapt to changing ECG signal quality. The
% first pass through y[n] uses these thresholds to classify the each non-zero sample
% (CURRENTPEAK) as either signal or noise:
% If CURRENTPEAK > THR_SIG, that location is identified as a QRS complex
% candidate?and the signal level (SIG_LEV) is updated:
% SIG _ LEV = 0.125 CURRENTPEAK + 0.875?SIG _ LEV

% If THR_NOISE < CURRENTPEAK < THR_SIG, then that location is identified as a
% Noise peak?and the noise level (NOISE_LEV) is updated:
% NOISE _ LEV = 0.125CURRENTPEAK + 0.875?NOISE _ LEV
% Based on new estimates of the signal and noise levels (SIG_LEV and NOISE_LEV,
% respectively) at that point in the ECG, the thresholds are adjusted as follows:
% THR _ SIG = NOISE _ LEV + 0.25 ?(SIG _ LEV-NOISE _ LEV )
% THR _ NOISE = 0.5?(THR _ SIG)
% These adjustments lower the threshold gradually in signal segments that are deemed to
% be of poorer quality.

% c) SEARCHBACK FOR MISSED QRS COMPLEXES - In the thresholding step above, if
% CURRENTPEAK < THR_SIG, the peak is deemed not to have resulted from a QRS
% complex. If however, an unreasonably long period has expired without an abovethreshold
% peak, the algorithm will assume a QRS has been missed and perform a
% searchback. This limits the number of false negatives. The minimum time used to trigger
% a searchback is 1.66 times the current R peak to R peak time period (called the RR
% interval). This value has a physiological origin - the time value between adjacent
% heartbeats cannot change more quickly than this. The missed QRS complex is assumed
% to occur at the location of the highest peak in the interval that lies between THR_SIG and
% THR_NOISE. In this algorithm, two average RR intervals are stored,the first RR interval is
% calculated as an average of the last eight QRS locations in order to adapt to changing heart
% rate and the second RR interval mean is the mean
% of the most regular RR intervals . The threshold is lowered if the heart rate is not regular
% to improve detection.

% d) ELIMINATION OF MULTIPLE DETECTIONS WITHIN REFRACTORY PERIOD - It is
% impossible for a legitimate QRS complex to occur if it lies within 200ms after a previously
% detected one. This constraint is a physiological one ?due to the refractory period during
% which ventricular depolarization cannot occur despite a stimulus[1]. As QRS complex
% candidates are generated, the algorithm eliminates such physically impossible events,
% thereby reducing false positives.

% e) T WAVE DISCRIMINATION - Finally, if a QRS candidate occurs after the 200ms
% refractory period but within 360ms of the previous QRS, the algorithm determines
% whether this is a genuine QRS complex of the next heartbeat or an abnormally prominent
% T wave. This decision is based on the mean slope of the waveform at that position. A slope of
% less than one half that of the previous QRS complex is consistent with the slower
% changing behaviour of a T wave ?otherwise, it becomes a QRS detection.
% Extra concept : beside the points mentioned in the paper, this code also
% checks if the occured peak which is less than 360 msec latency has also a
% latency less than 0,5*mean_RR if yes this is counted as noise

% f) In the final stage , the output of R waves detected in smoothed signal is analyzed and double
% checked with the help of the output of the bandpass signal to improve
% detection and find the original index of the real R waves on the raw ecg
% signal

%% References :

%[1]PAN.J, TOMPKINS. W.J,"A Real-Time QRS Detection Algorithm" IEEE
%TRANSACTIONS ON BIOMEDICAL ENGINEERING, VOL. BME-32, NO. 3, MARCH 1985.

%% Author : Hooman Sedghamiz
% Linkoping university
% email : hoose792@student.liu.se
% hooman.sedghamiz@medel.com

% Any direct or indirect use of this code should be referenced
% Copyright march 2014
%%
if ~isvector(ecg)
  error('ecg must be a row or column vector');
end

if nargin < 3
    gr = 1;   % on default the function always plots
end
ecg = ecg(:); % vectorize

%% Initialize
qrs_c =[]; %amplitude of R
qrs_i =[]; %index
SIG_LEV = 0;
nois_c =[];
nois_i =[];
delay = 0;
skip = 0; % becomes one when a T wave is detected
not_nois = 0; % it is not noise when not_nois = 1
selected_RR =[]; % Selected RR intervals
m_selected_RR = 0;
mean_RR = 0;
qrs_i_raw =[];
qrs_amp_raw=[];
ser_back = 0;
test_m = 0;
SIGL_buf = [];
NOISL_buf = [];
THRS_buf = [];
SIGL_buf1 = [];
NOISL_buf1 = [];
THRS_buf1 = [];

%% Plot differently based on filtering settings
if gr
 if fs == 200
  figure,  ax(1)=subplot(321);plot(ecg);axis tight;title('Raw ECG Signal');
 else
  figure,  ax(1)=subplot(3,2,[1 2]);plot(ecg);axis tight;title('Raw ECG Signal');
 end
end
%% Noise cancelation(Filtering) % Filters (Filter in between 5-15 Hz)
if fs == 200
%% Low Pass Filter  H(z) = ((1 - z^(-6))^2)/(1 - z^(-1))^2
b = [1 0 0 0 0 0 -2 0 0 0 0 0 1];
a = [1 -2 1];
h_l = filter(b,a,[1 zeros(1,12)]);
ecg_l = conv (ecg ,h_l);
ecg_l = ecg_l/ max( abs(ecg_l));
delay = 6; %based on the paper
if gr
ax(2)=subplot(322);plot(ecg_l);axis tight;title('Low pass filtered');
end
%% High Pass filter H(z) = (-1+32z^(-16)+z^(-32))/(1+z^(-1))
b = [-1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 32 -32 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1];
a = [1 -1];
h_h = filter(b,a,[1 zeros(1,32)]);
ecg_h = conv (ecg_l ,h_h);
ecg_h = ecg_h/ max( abs(ecg_h));
delay = delay + 16; % 16 samples for highpass filtering
if gr
ax(3)=subplot(323);plot(ecg_h);axis tight;title('High Pass Filtered');
end
else
%% bandpass filter for Noise cancelation of other sampling frequencies(Filtering)
f1=5; %cuttoff low frequency to get rid of baseline wander
f2=15; %cuttoff frequency to discard high frequency noise
Wn=[f1 f2]*2/fs; % cutt off based on fs
N = 3; % order of 3 less processing
[a,b] = butter(N,Wn); %bandpass filtering
ecg_h = filtfilt(a,b,ecg);
ecg_h = ecg_h/ max( abs(ecg_h));
if gr
ax(3)=subplot(323);plot(ecg_h);axis tight;title('Band Pass Filtered');
end
end
%% derivative filter H(z) = (1/8T)(-z^(-2) - 2z^(-1) + 2z + z^(2))
h_d = [-1 -2 0 2 1]*(1/8);%1/8*fs
ecg_d = conv (ecg_h ,h_d);
ecg_d = ecg_d/max(ecg_d);
delay = delay + 2; % delay of derivative filter 2 samples
if gr
ax(4)=subplot(324);plot(ecg_d);axis tight;title('Filtered with the derivative filter');
end
%% Squaring nonlinearly enhance the dominant peaks
ecg_s = ecg_d.^2;
if gr
ax(5)=subplot(325);plot(ecg_s);axis tight;title('Squared');
end

%% Moving average Y(nt) = (1/N)[x(nT-(N - 1)T)+ x(nT - (N - 2)T)+...+x(nT)]
ecg_m = conv(ecg_s ,ones(1 ,round(0.150*fs))/round(0.150*fs));
delay = delay + 15;

if gr
ax(6)=subplot(326);plot(ecg_m);axis tight;title('Averaged with 30 samples length,Black noise,Green Adaptive Threshold,RED Sig Level,Red circles QRS adaptive threshold');
axis tight;
end

%% Fiducial Mark
% Note : a minimum distance of 40 samples is considered between each R wave
% since in physiological point of view no RR wave can occur in less than
% 200 msec distance
[pks,locs] = findpeaks(ecg_m,'MINPEAKDISTANCE',round(0.2*fs));

%% initialize the training phase (2 seconds of the signal) to determine the THR_SIG and THR_NOISE
THR_SIG = max(ecg_m(1:2*fs))*1/3; % 0.25 of the max amplitude
THR_NOISE = mean(ecg_m(1:2*fs))*1/2; % 0.5 of the mean signal is considered to be noise
SIG_LEV= THR_SIG;
NOISE_LEV = THR_NOISE;

%% Initialize bandpath filter threshold(2 seconds of the bandpass signal)
THR_SIG1 = max(ecg_h(1:2*fs))*1/3; % 0.25 of the max amplitude
THR_NOISE1 = mean(ecg_h(1:2*fs))*1/2; %
SIG_LEV1 = THR_SIG1; % Signal level in Bandpassed filter
NOISE_LEV1 = THR_NOISE1; % Noise level in Bandpassed filter
%% Thresholding and online desicion rule

for i = 1 : length(pks)

   %% locate the corresponding peak in the filtered signal
    if locs(i)-round(0.150*fs)>= 1 && locs(i)<= length(ecg_h)
          [y_i x_i] = max(ecg_h(locs(i)-round(0.150*fs):locs(i)));
       else
          if i == 1
            [y_i x_i] = max(ecg_h(1:locs(i)));
            ser_back = 1;
          elseif locs(i)>= length(ecg_h)
            [y_i x_i] = max(ecg_h(locs(i)-round(0.150*fs):end));
          end

     end

  %% update the heart_rate (Two heart rate means one the moste recent and the other selected)
    if length(qrs_c) >= 9

        diffRR = diff(qrs_i(end-8:end)); %calculate RR interval
        mean_RR = mean(diffRR); % calculate the mean of 8 previous R waves interval
        comp =qrs_i(end)-qrs_i(end-1); %latest RR
        if comp <= 0.92*mean_RR || comp >= 1.16*mean_RR
            % lower down thresholds to detect better in MVI
                THR_SIG = 0.5*(THR_SIG);
                %THR_NOISE = 0.5*(THR_SIG);
               % lower down thresholds to detect better in Bandpass filtered
                THR_SIG1 = 0.5*(THR_SIG1);
                %THR_NOISE1 = 0.5*(THR_SIG1);

        else
            m_selected_RR = mean_RR; %the latest regular beats mean
        end

    end

      %% calculate the mean of the last 8 R waves to make sure that QRS is not
       % missing(If no R detected , trigger a search back) 1.66*mean

       if m_selected_RR
           test_m = m_selected_RR; %if the regular RR availabe use it
       elseif mean_RR && m_selected_RR == 0
           test_m = mean_RR;
       else
           test_m = 0;
       end

    if test_m
          if (locs(i) - qrs_i(end)) >= round(1.66*test_m)% it shows a QRS is missed
              [pks_temp,locs_temp] = max(ecg_m(qrs_i(end)+ round(0.200*fs):locs(i)-round(0.200*fs))); % search back and locate the max in this interval
              locs_temp = qrs_i(end)+ round(0.200*fs) + locs_temp -1; %location

              if pks_temp > THR_NOISE
               qrs_c = [qrs_c pks_temp];
               qrs_i = [qrs_i locs_temp];

               % find the location in filtered sig
               if locs_temp <= length(ecg_h)
                [y_i_t x_i_t] = max(ecg_h(locs_temp-round(0.150*fs):locs_temp));
               else
                [y_i_t x_i_t] = max(ecg_h(locs_temp-round(0.150*fs):end));
               end
               % take care of bandpass signal threshold
               if y_i_t > THR_NOISE1

                      qrs_i_raw = [qrs_i_raw locs_temp-round(0.150*fs)+ (x_i_t - 1)];% save index of bandpass
                      qrs_amp_raw =[qrs_amp_raw y_i_t]; %save amplitude of bandpass
                      SIG_LEV1 = 0.25*y_i_t + 0.75*SIG_LEV1; %when found with the second thres
               end

               not_nois = 1;
               SIG_LEV = 0.25*pks_temp + 0.75*SIG_LEV ;  %when found with the second threshold
             end

          else
              not_nois = 0;

          end
    end

    %%  find noise and QRS peaks
    if pks(i) >= THR_SIG

                 % if a QRS candidate occurs within 360ms of the previous QRS
                 % ,the algorithm determines if its T wave or QRS
                 if length(qrs_c) >= 3
                      if (locs(i)-qrs_i(end)) <= round(0.3600*fs)
                        Slope1 = mean(diff(ecg_m(locs(i)-round(0.075*fs):locs(i)))); %mean slope of the waveform at that position
                        Slope2 = mean(diff(ecg_m(qrs_i(end)-round(0.075*fs):qrs_i(end)))); %mean slope of previous R wave
                             if abs(Slope1) <= abs(0.5*(Slope2))  % slope less then 0.5 of previous R
                                 nois_c = [nois_c pks(i)];
                                 nois_i = [nois_i locs(i)];
                                 skip = 1; % T wave identification
                                 % adjust noise level in both filtered and
                                 % MVI
                                 NOISE_LEV1 = 0.125*y_i + 0.875*NOISE_LEV1;
                                 NOISE_LEV = 0.125*pks(i) + 0.875*NOISE_LEV;
                             else
                                 skip = 0;
                             end

                      end
                 end

        if skip == 0  % skip is 1 when a T wave is detected
        qrs_c = [qrs_c pks(i)];
        qrs_i = [qrs_i locs(i)];

        % bandpass filter check threshold
         if y_i >= THR_SIG1
                        if ser_back
                           qrs_i_raw = [qrs_i_raw x_i];  % save index of bandpass
                        else
                           qrs_i_raw = [qrs_i_raw locs(i)-round(0.150*fs)+ (x_i - 1)];% save index of bandpass
                        end
                           qrs_amp_raw =[qrs_amp_raw y_i];% save amplitude of bandpass
          SIG_LEV1 = 0.125*y_i + 0.875*SIG_LEV1;% adjust threshold for bandpass filtered sig
         end

        % adjust Signal level
        SIG_LEV = 0.125*pks(i) + 0.875*SIG_LEV ;
        end

    elseif THR_NOISE <= pks(i) && pks(i)<THR_SIG

         %adjust Noise level in filtered sig
         NOISE_LEV1 = 0.125*y_i + 0.875*NOISE_LEV1;
         %adjust Noise level in MVI
         NOISE_LEV = 0.125*pks(i) + 0.875*NOISE_LEV;

    elseif pks(i) < THR_NOISE
        nois_c = [nois_c pks(i)];
        nois_i = [nois_i locs(i)];

        % noise level in filtered signal
        NOISE_LEV1 = 0.125*y_i + 0.875*NOISE_LEV1;
        %end

         %adjust Noise level in MVI
        NOISE_LEV = 0.125*pks(i) + 0.875*NOISE_LEV;

    end

    %% adjust the threshold with SNR
    if NOISE_LEV ~= 0 || SIG_LEV ~= 0
        THR_SIG = NOISE_LEV + 0.25*(abs(SIG_LEV - NOISE_LEV));
        THR_NOISE = 0.5*(THR_SIG);
    end

    % adjust the threshold with SNR for bandpassed signal
    if NOISE_LEV1 ~= 0 || SIG_LEV1 ~= 0
        THR_SIG1 = NOISE_LEV1 + 0.25*(abs(SIG_LEV1 - NOISE_LEV1));
        THR_NOISE1 = 0.5*(THR_SIG1);
    end

% take a track of thresholds of smoothed signal
SIGL_buf = [SIGL_buf SIG_LEV];
NOISL_buf = [NOISL_buf NOISE_LEV];
THRS_buf = [THRS_buf THR_SIG];

% take a track of thresholds of filtered signal
SIGL_buf1 = [SIGL_buf1 SIG_LEV1];
NOISL_buf1 = [NOISL_buf1 NOISE_LEV1];
THRS_buf1 = [THRS_buf1 THR_SIG1];

 skip = 0; %reset parameters
 not_nois = 0; %reset parameters
 ser_back = 0;  %reset bandpass param
end

if gr
hold on,scatter(qrs_i,qrs_c,'m');
hold on,plot(locs,NOISL_buf,'--k','LineWidth',2);
hold on,plot(locs,SIGL_buf,'--r','LineWidth',2);
hold on,plot(locs,THRS_buf,'--g','LineWidth',2);
if ax(:)
linkaxes(ax,'x');
zoom on;
end
end

%% overlay on the signals
if gr
figure,az(1)=subplot(311);plot(ecg_h);title('QRS on Filtered Signal');axis tight;
hold on,scatter(qrs_i_raw,qrs_amp_raw,'m');
hold on,plot(locs,NOISL_buf1,'LineWidth',2,'Linestyle','--','color','k');
hold on,plot(locs,SIGL_buf1,'LineWidth',2,'Linestyle','-.','color','r');
hold on,plot(locs,THRS_buf1,'LineWidth',2,'Linestyle','-.','color','g');
az(2)=subplot(312);plot(ecg_m);title('QRS on MVI signal and Noise level(black),Signal Level (red) and Adaptive Threshold(green)');axis tight;
hold on,scatter(qrs_i,qrs_c,'m');
hold on,plot(locs,NOISL_buf,'LineWidth',2,'Linestyle','--','color','k');
hold on,plot(locs,SIGL_buf,'LineWidth',2,'Linestyle','-.','color','r');
hold on,plot(locs,THRS_buf,'LineWidth',2,'Linestyle','-.','color','g');
az(3)=subplot(313);plot(ecg-mean(ecg));title('Pulse train of the found QRS on ECG signal');axis tight;
line(repmat(qrs_i_raw,[2 1]),repmat([min(ecg-mean(ecg))/2; max(ecg-mean(ecg))/2],size(qrs_i_raw)),'LineWidth',2.5,'LineStyle','-.','Color','r');
linkaxes(az,'x');
zoom on;
end
end


function [vals, c_index_1, c_index_2, y_1, y_2] = price_2d_ctmc( S_0s, T, r, rho, sigmas, qs, params, contractParams, M)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Pricing Function for European/Bermudan/Barrier Options using CTMC approximation method
% Models Supported: 2D Diffusions
% Returns: price of contract
% Author: Justin Lars Kirkby
%
% References:  (1) A General Continuous Time Markov Chain Approximation for
%               Multi-Asset option pricing with systems of correlated diffusions,
%               Applied Math. and Comput., 2020 (JL Kirkby, Duy Nguyen, Dang Nguyen)
%
% ----------------------
% Model Params
% ----------------------
% S_0s   = initial asset prices
% T      = time remaining until maturity (in years, e.g. T=1)
% r      = interest rate (e.g. 0.05)
% rho    = correlation between brownian motions
% sigmas = volatilities per asset
% qs     = div yeilds per asset
% M      = num monitoring points for Barrier/Bermudan (also controls num steps for multi-step European pricing)
%
% ----------------------
% Contract Params  (contractParams)
% ----------------------
%
% contractParams.payoff_type:
%
% 1: Linear, G = S_1  (linear payoff in first underlying)
% 2: Linear, G = S_2  (linear payoff in second underlying)
% 3: Exchange, G = (S_1 - S_2)^+
% 4: Spread,  G = (S_1 - S_2 - K)^+   (NOTE: must set strike, K)
% 5: Geometric Basket Call / Put,  G = (sqrt(S_1) * sqrt(S_2) - K)^+  (for the call)
% 6: Arithmetic Basket Call / Put,  G = (sqrt(S_1) * sqrt(S_2) - K)^+  (for the call)
% 7: Call-on-Max and Put-on-Min, Gcall = (max(S_1,S_2) - K)^+ , Gput = (K - min(S_1,S_2))^+
% 8: Call/put on just S_2, G = (S_2 - K)^+  (for the call)
% 9: Best-of / Worst-of,  G = max(S_1,S_2), G = min(S_1,S_2)
%
% contractParams.contract:
%
% 1: European, Single Step Pricing
% 2: European, Multi Step Pricing (M above controls number of steps)
% 3: Bermudan (M above controls number of monitoring points)
% 4: Barrier (M above controls number of monitoring points)
%
% Note: for barrier option, set:
%       contractParams.barriers_1 = lower/upper barriers on first asset  (e.g. [0 50])
%       contractParams.barriers_2 = lower/upper barriers on second asset  (e.g. [0 50000000000] to disable barrier on S_2)
%
% ----------------------
% Numerical (CTMC) Params
% ----------------------
% params = CTMC parameters
% params.m_0 = num CTCM states
% params.num_devs = num std devs used in the grid
% params.gridMethod = choose the grid method (several RnD versions)
% params.GridMultParam = non-uniformity param, in (0,1)

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

if nargin < 9
    M = 1; % M is only needed for
end
dt = T/M;

contract = contractParams.contract;

if contract == 1  % European
    dt = 1; M = 1;
end

method = 4;
num_devs = params.num_devs;
m_0 = params.m_0;
GridMultParam = params.GridMultParam;
gridMethod = params.gridMethod;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

drifts = r - qs;

R = [1 rho; rho 1];  % Correlation Matrix
[ L, D, C, Cinv ] = get_transform_matrices_2d( R, method );

% Now Define New Uncorrelated System  (the dc underscore)
[drift_dc, sigma_dc ] = decorrelate(sigmas, drifts, C, D );

[Ls_dc, Rs_dc ] = get_CTMC_decorr_boundaries(sigmas, C, T, 0, sigma_dc, num_devs);
Y_0s = [0 0];

% Form CTMC 1
center = Y_0s(1);
mu_func = @(s) drift_dc(1)*[s>-100000];
sig_func = @(s) sigma_dc(1)*[s>-100000];
[Q, y_1, c_index_1] = Q_Matrix(m_0, mu_func,sig_func,Ls_dc(1),Rs_dc(1),gridMethod,center, GridMultParam);
P1 = expm(Q*dt);

% Form CTMC 2
center = Y_0s(2);
mu_func = @(s) drift_dc(2)*[s>-100000];
sig_func = @(s) sigma_dc(2)*[s>-100000];
[Q, y_2, c_index_2] = Q_Matrix(m_0, mu_func,sig_func,Ls_dc(2),Rs_dc(2),gridMethod,center, GridMultParam);
P2 = expm(Q*dt);

G = get_payoff_G_matrix_from_ygrid_2d( y_1, y_2, S_0s, sigmas, rho, contractParams);

if contract == 1  % European, Price by single step
    vals = exp(-r*T)*P1*G*P2.';

elseif contract == 2  % European, Price By Multi Step
    vals = G;
    for m=M-1:-1:0
        vals = exp(-r*dt)*P1*vals*P2.';
    end

elseif contract == 3  % Bermudan
    vals = G;
    for m=M-1:-1:0
        vals = max(exp(-r*dt)*P1*vals*P2.', G);
    end

elseif contract == 4 % Barrier
    b1 = contractParams.barriers_1; L1 = b1(1); U1 = b1(2);
    b2 = contractParams.barriers_2; L2 = b2(1); U2 = b2(2);
    B = ones(m_0, m_0);
    for i = 1:m_0
        y1 = y_1(i);
        S1 = S_0s(1)*exp(sigmas(1)*y1);
        if S1 < L1 || S1 > U1
            B(i,:) = 0;
        else
            for j = 1:m_0
                y2 = y_2(j);
                S2 = S_0s(2)*exp(sigmas(2)*(y2 + rho*y1));
                if S2 < L2 || S2 > U2
                    B(i,j) = 0;
                end
            end
        end
    end
    vals = G.*B;
    for m=M-1:-1:0
        vals = exp(-r*dt)*(B.*P1*vals*P2.');
    end
end

end


function [ value, ifault ] = ppchi2 ( p, v, g )

%*****************************************************************************80
%
%% PPCHI2 evaluates the percentage points of the Chi-squared PDF.
%
%  Discussion
%
%    Incorporates the suggested changes in AS R85 (vol.40(1),
%    pages 233-5, 1991) which should eliminate the need for the limited
%    range for P, though these limits have not been removed
%    from the routine.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2008
%
%  Author:
%
%    Original FORTRAN77 version by Donald Best, DE Roberts.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Donald Best, DE Roberts,
%    Algorithm AS 91:
%    The Percentage Points of the Chi-Squared Distribution,
%    Applied Statistics,
%    Volume 24, Number 3, 1975, pages 385-390.
%
%  Parameters:
%
%    Input, real P,  value of the chi-squared cumulative
%    probability density function.
%    0.000002 <= P <= 0.999998.
%
%    Input, real V, the parameter of the chi-squared probability
%    density function.
%    0 < V.
%
%    Input, real G, the value of log ( Gamma ( V / 2 ) ).
%
%    Output, real VALUE, the value of the chi-squared random
%    deviate with the property that the probability that a chi-squared random
%    deviate with parameter V is less than or equal to PPCHI2 is P.
%
%    Output, integer IFAULT, is nonzero if an error occurred.
%    0, no error.
%    1, P is outside the legal range.
%    2, V is not positive.
%    3, an error occurred in GAMMAD.
%    4, the result is probably as accurate as the machine will allow.
%
  aa = 0.6931471806;
  c1 = 0.01;
  c2 = 0.222222;
  c3 = 0.32;
  c4 = 0.4;
  c5 = 1.24;
  c6 = 2.2;
  c7 = 4.67;
  c8 = 6.66;
  c9 = 6.73;
  c10 = 13.32;
  c11 = 60.0;
  c12 = 70.0;
  c13 = 84.0;
  c14 = 105.0;
  c15 = 120.0;
  c16 = 127.0;
  c17 = 140.0;
  c18 = 175.0;
  c19 = 210.0;
  c20 = 252.0;
  c21 = 264.0;
  c22 = 294.0;
  c23 = 346.0;
  c24 = 420.0;
  c25 = 462.0;
  c26 = 606.0;
  c27 = 672.0;
  c28 = 707.0;
  c29 = 735.0;
  c30 = 889.0;
  c31 = 932.0;
  c32 = 966.0;
  c33 = 1141.0;
  c34 = 1182.0;
  c35 = 1278.0;
  c36 = 1740.0;
  c37 = 2520.0;
  c38 = 5040.0;
  e = 0.5E-06;
  maxit = 20;
  pmax = 0.999998;
  pmin = 0.000002;
%
%  Test arguments and initialize.
%
  value = - 1.0;

  if ( p < pmin || pmax < p )
    ifault = 1;
    return
  end

  if ( v <= 0.0 )
    ifault = 2;
    return
  end

  ifault = 0;
  xx = 0.5 * v;
  c = xx - 1.0;
%
%  Starting approximation for small chi-squared
%
  if ( v < - c5 * log ( p ) )

    ch = ( p * xx * exp ( g + xx * aa ) )^( 1.0 / xx );

    if ( ch < e )
      value = ch;
      return
    end
%
%  Starting approximation for V less than or equal to 0.32
%
  elseif ( v <= c3 )

    ch = c4;
    a = log ( 1.0 - p );

    while ( 1 )

      q = ch;
      p1 = 1.0 + ch * ( c7 + ch );
      p2 = ch * ( c9 + ch * ( c8 + ch ) );

      t = - 0.5 + (c7 + 2.0 * ch ) / p1 ...
        - ( c9 + ch * ( c10 + 3.0 * ch ) ) / p2

      ch = ch - ( 1.0 - exp ( a + g + 0.5 * ch + c * aa ) * p2 / p1) / t;

      if ( abs ( q / ch - 1.0 ) <= c1 )
        break
      end

    end

  else
%
%  Call to algorithm AS 111 - note that P has been tested above.
%  AS 241 could be used as an alternative.
%
   [ x, ifault ] = ppnd ( p );
%
%  Starting approximation using Wilson and Hilferty estimate
%
    p1 = c2 / v;
    ch = v * ( x * sqrt ( p1 ) + 1.0 - p1)^3;
%
%  Starting approximation for P tending to 1.
%
    if ( c6 * v + 6.0 < ch )
       ch = - 2.0 * ( log ( 1.0 - p ) - c * log ( 0.5 * ch ) + g );
    end

  end
%
%  Call to algorithm AS 239 and calculation of seven term
%  Taylor series
%
  for i = 1 : maxit

    q = ch;
    p1 = 0.5 * ch;
    [ temp, if1 ] = gammad ( p1, xx );
    p2 = p - temp;

    if ( if1 ~= 0 )
      ifault = 3;
      return
    end

    t = p2 * exp ( xx * aa + g + p1 - c * log ( ch ) );
    b = t / ch;
    a = 0.5 * t - b * c;
    s1 = ( c19 + a * ( c17 + a * ( c14 + a * ( c13 + a * ( c12 + ...
      c11 * a ))))) / c24;
    s2 = ( c24 + a * ( c29 + a * ( c32 + a * ( c33 + c35 * a )))) / c37;
    s3 = ( c19 + a * ( c25 + a * ( c28 + c31 * a ))) / c37;
    s4 = ( c20 + a * ( c27 + c34 * a) + c * ( c22 + a * ( c30 + c36 * a ))) ...
      / c38;
    s5 = ( c13 + c21 * a + c * ( c18 + c26 * a )) / c37;
    s6 = ( c15 + c * ( c23 + c16 * c )) / c38;
    ch = ch + t * ( 1.0 + 0.5 * t * s1 - b * c * ( s1 - b * ...
      ( s2 - b * ( s3 - b * ( s4 - b * ( s5 - b * s6 ))))));

    if ( e < abs ( q / ch - 1.0 ) )
       value = ch;
       return
    end

  end

  ifault = 4;
  value = ch;

  return
end


function imf = bwtfilter(im,f)
% function imf = bwtfilter(im,f)
%
% BWT decimated filtering
% Version 1.2
%
% This is faster than doing the complete filtering operation with conv2 and
% then decimating the result
%
% Arguments:
%  im: A square image, with side length a multiple of 3
%  f:  A 3x3 filter
%
% Result:
%  imf: im is filtered with f, and the result is decimated, taking every
%       3rd value in x and y. This code is equivalent to:
%       imf = conv2(im, rot90(rot90(f)),'same');
%       imf = imf(2:3:end,2:3:end);
%
% Citation:
%  Willmore B, Prenger RJ, Wu MC and Gallant JL (2008). The Berkeley
%  Wavelet Transform: A biologically-inspired orthogonal wavelet transform.
%  Neural Computation 20:6, 1537-1564
%
% The article is available at:
%  <http://dx.doi.org/10.1162/neco.2007.05-07-513>
%
% Copyright (c) 2008 Ben Willmore
%
% Permission is hereby granted, free of charge, to any person
% obtaining a copy of this software and associated documentation
% files (the "Software"), to deal in the Software without
% restriction, including without limitation the rights to use,
% copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following
% conditions:
%
% The above copyright notice and this permission notice shall be
% included in all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
% EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
% OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
% NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
% HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
% WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
% OTHER DEALINGS IN THE SOFTWARE.

sz = size(im);

if (length(sz) ~= 2) || (sz(1) ~= sz(2))
  disp('Input must be square');
  decomp = nan;
  return;
end

sz = sz(1);
ssz = sz/3;

if ( (ssz-floor(ssz)) > abs(ssz)*eps )
  fprintf('Side length must be a multiple of 3');
  decomp = nan;
  return;
end

f_rep = repmat(f,ssz,ssz);

imdot = im .* f_rep;

imf = zeros(ssz);

for yy = 1:3
  for xx = 1:3
    imf = imf + imdot(yy:3:end,xx:3:end);
  end
end


function M = stiefelfactory(n, p, k, gpuflag)
% Returns a manifold structure to optimize over orthonormal matrices.
%
% function M = stiefelfactory(n, p)
% function M = stiefelfactory(n, p, k)
% function M = stiefelfactory(n, p, k, gpuflag)
%
% The Stiefel manifold is the set of orthonormal nxp matrices. If k
% is larger than 1, this is the Cartesian product of the Stiefel manifold
% taken k times. The metric is such that the manifold is a Riemannian
% submanifold of R^nxp equipped with the usual trace inner product, that
% is, it is the usual metric.
%
% Points are represented as matrices X of size n x p x k (or n x p if k=1,
% which is the default) such that each n x p matrix is orthonormal,
% i.e., X'*X = eye(p) if k = 1, or X(:, :, i)' * X(:, :, i) = eye(p) for
% i = 1 : k if k > 1. Tangent vectors are represented as matrices the same
% size as points.
%
% The default retraction is QR-based: it is only a first-order retraction.
% To use the polar retraction (which is second order), run
%    M.retr = M.retr_polar;
% after creating M with this factory. This can be reverted with
%    M.retr = M.retr_qr;
% If used, you may also want to update M.invretr similarly.
%
% Set gpuflag = true to have points, tangent vectors and ambient vectors
% stored on the GPU. If so, computations can be done on the GPU directly.
%
% By default, k = 1 and gpuflag = false.
%
% See also: grassmannfactory rotationsfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%  July  5, 2013 (NB) : Added ehess2rhess.
%  Jan. 27, 2014 (BM) : Bug in ehess2rhess corrected.
%  June 24, 2014 (NB) : Added true exponential map and changed the randvec
%                       function so that it now returns a globally
%                       normalized vector, not a vector where each
%                       component is normalized (this only matters if k>1).
%  July 17, 2018 (NB) : Now both QR (default) and polar retractions are
%                       directly accessible, and their inverses are also
%                       implemented.
%  Aug.  2, 2018 (NB) : Added GPU support: just set gpuflag = true.
%  June 18, 2019 (NB) : Using qr_unique for retr and rand.
%  July  9, 2019 (NB) : Added a comment about QR retraction being first
%                       order only.
%  Jan.  8, 2021 (NB) : Added tangent2ambient+tangent2ambient_is_identity.

    assert(n >= p, 'The dimension n must be larger than the dimension p.');

    if ~exist('k', 'var') || isempty(k)
        k = 1;
    end
    if ~exist('gpuflag', 'var') || isempty(gpuflag)
        gpuflag = false;
    end

    % If gpuflag is active, new arrays (e.g., via rand, randn, zeros, ones)
    % are created directly on the GPU; otherwise, they are created in the
    % usual way (in double precision).
    if gpuflag
        array_type = 'gpuArray';
    else
        array_type = 'double';
    end

    if k == 1
        M.name = @() sprintf('Stiefel manifold St(%d, %d)', n, p);
    elseif k > 1
        M.name = @() sprintf('Product Stiefel manifold St(%d, %d)^%d', n, p, k);
    else
        error('k must be an integer no less than 1.');
    end

    M.dim = @() k*(n*p - .5*p*(p+1));

    M.inner = @(x, d1, d2) d1(:).'*d2(:);

    M.norm = @(x, d) norm(d(:));

    M.dist = @(x, y) error('stiefel.dist not implemented yet.');

    M.typicaldist = @() sqrt(p*k);

    M.proj = @projection;
    function Up = projection(X, U)

        XtU = multiprod(multitransp(X), U);
        symXtU = multisym(XtU);
        Up = U - multiprod(X, symXtU);

% The code above is equivalent to, but faster than, the code below.
%
%     Up = zeros(size(U));
%     function A = sym(A), A = .5*(A+A'); end
%     for i = 1 : k
%         Xi = X(:, :, i);
%         Ui = U(:, :, i);
%         Up(:, :, i) = Ui - Xi*sym(Xi'*Ui);
%     end

    end

    M.tangent = M.proj;

    M.tangent2ambient_is_identity = true;
    M.tangent2ambient = @(X, U) U;

    % For Riemannian submanifolds, converting a Euclidean gradient into a
    % Riemannian gradient amounts to an orthogonal projection.
    M.egrad2rgrad = M.proj;

    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(X, egrad, ehess, H)
        XtG = multiprod(multitransp(X), egrad);
        symXtG = multisym(XtG);
        HsymXtG = multiprod(H, symXtG);
        rhess = projection(X, ehess - HsymXtG);
    end

    M.retr_qr = @retraction_qr;
    function Y = retraction_qr(X, U, t)
        % It is necessary to call qr_unique rather than simply qr to ensure
        % this is a retraction, to avoid spurious column sign flips.
        if nargin < 3
            Y = qr_unique(X + U);
        else
            Y = qr_unique(X + t*U);
        end
    end

    M.invretr_qr = @invretr_qr;
    function U = invretr_qr(X, Y)
        XtY = multiprod(multitransp(X), Y);
        R = zeros(p, p, k, array_type);
        H = 2*eye(p, array_type);
        for kk = 1 : k
            % For each slice, assuming the inverse retraction is well
            % defined for the given inputs, we have:
            %   X + U = YR
            % Left multiply with X' to get
            %   I + X'U = X'Y R
            % Since X'U is skew symmetric for a tangent vector U at X, add
            % up this equation with its transpose to get:
            %   2I = (X'Y) R + R' (X'Y)'
            % Contrary to the polar factorization, here R is not symmetric
            % but it is upper triangular. As a result, this is not a
            % Sylvester equation and we must solve it differently.
            R(:, :, kk) = solve_for_triu(XtY(:, :, kk), H);
            % Then,
            %   U = YR - X
            % which is what we compute below.
        end
        U = multiprod(Y, R) - X;
    end

    M.retr_polar = @retraction_polar;
    function Y = retraction_polar(X, U, t)
        if nargin < 3
            Y = X + U;
        else
            Y = X + t*U;
        end
        for kk = 1 : k
            [u, s, v] = svd(Y(:, :, kk), 'econ'); %#ok
            Y(:, :, kk) = u*v';
        end
    end

    M.invretr_polar = @invretr_polar;
    function U = invretr_polar(X, Y)
        XtY = multiprod(multitransp(X), Y);
        MM = zeros(p, p, k, array_type);
        H = 2*eye(p, array_type);
        for kk = 1 : k
            % For each slice, assuming the inverse retraction is well
            % defined for the given inputs, we have:
            %   X + U = YM
            % Left multiply with X' to get
            %   I + X'U = X'Y M
            % Since X'U is skew symmetric for a tangent vector U at X, add
            % up this equation with its transpose to get:
            %   2I = (X'Y) M + M' (X'Y)'
            %      = (X'Y) M + M (X'Y)'   since M is symmetric.
            % Solve for M symmetric with a call to sylvester:
            MM(:, :, kk) = sylvester_nochecks(XtY(:, :, kk), XtY(:, :, kk)', H);
            % Note that the above is really a Lyapunov equation: it could
            % be solved faster by exploiting the fact the same matrix
            % appears twice on the left, with one the transpose of the
            % other. Then,
            %   U = YM - X
            % which is what we compute below.
        end
        U = multiprod(Y, MM) - X;
    end

    % By default, we use the QR retraction
    M.retr = M.retr_qr;
    M.invretr = M.invretr_qr;

    M.exp = @exponential;
    function Y = exponential(X, U, t)
        if nargin == 2
            tU = U;
        else
            tU = t*U;
        end
        Y = zeros(size(X), array_type);
        I = eye(p, array_type);
        Z = zeros(p, array_type);
        for kk = 1 : k
            % From a formula by Ross Lippert, Example 5.4.2 in AMS08.
            Xkk = X(:, :, kk);
            Ukk = tU(:, :, kk);
            Y(:, :, kk) = [Xkk Ukk] * ...
                         expm([Xkk'*Ukk , -Ukk'*Ukk ; I , Xkk'*Ukk]) * ...
                         [ expm(-Xkk'*Ukk) ; Z ];
        end

    end

    M.hash = @(X) ['z' hashmd5(X(:))];

    M.rand = @() qr_unique(randn(n, p, k, array_type));

    M.randvec = @randomvec;
    function U = randomvec(X)
        U = projection(X, randn(n, p, k, array_type));
        U = U / norm(U(:));
    end

    M.lincomb = @matrixlincomb;

    M.zerovec = @(x) zeros(n, p, k, array_type);

    M.transp = @(x1, x2, d) projection(x2, d);

    M.vec = @(x, u_mat) u_mat(:);
    M.mat = @(x, u_vec) reshape(u_vec, [n, p, k]);
    M.vecmatareisometries = @() true;

    % Automatically convert a number of tools to support GPU.
    if gpuflag
        M = factorygpuhelper(M);
    end

end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%

% even or odd
 t1=0:4
 t2=0:-1:-4

 %even
 x1=t1.^2
 x2=t2.^2

%  odd
 y1=t1.^3
 y2=t2.^3

 %analysis in even and odd parts
 n=-5:5;
 u=(n>=0);
 stem(n,u);

 figure
 u_n=(n<=0)
 ue=1/2*(u+ u_n);
 stem(n,ue);

 figure
 uo=1/2*(u- u_n);
 stem(n,uo);

 figure
 stem(n,ue+uo)


%% meshDistMarch
% Below is a demonstration of the features of the |meshDistMarch| function

%%
clear; close all; clc;

%% Syntax
% |[d,seedIndex]=meshDistMarch(F,V,indStart,optionStruct);|

%% Description
% The meshDistMarch function can be used to compute distances on meshes.
% The distances can be used for points sampling on the mesh or for
% remeshing.
% The function can operate on edge descriptions or face descriptions.
% Therefore for volumetric meshes (e.g. consisting of tetrahedra or
% hexahedra) appropriate face or edge data should be computed first to
% formulate the input.
%
% Input:
% E: the edges or faces for the mesh. E.g. an nx2 edge matrix or an nxm
% face matrix (n faces, m corners per face)
% V: the vertices for the mesh
% indStart: indices for one or more points to compute distances from
% optionStruct.toleranceLevel : The tolerance level for convergence. 0 is
% the default.
% optionStruct.numSeeds: Defines the number of seeds to generate on the mesh. Default is equal to startInd.
% optionStruct.waitBarOn=0; %Turn on/off waitbar
% optionStruct.unitEdgeOn=1; %Turn on/off the use of unit edge lengths
%
% Output:
% d: distances (from the start/seed points) on the mesh vertices
% seedIndex: nearest seed (or start) point indices for each vertex, forming
% a quasi-Voronoi tesselation.
% If the second output is not requested the performance is enhanced.

%% Examples

%%
% Plot settings
cMapDist=flipud(igviridis(250));
[cMapIndices,scrambleIndices]=scramble(viridis(250),1); %Colormap

faceAlpha1=1;
faceAlpha2=0.65;
fontSize=25;
markerSize=50;

%% Example: Edge or graph data

%%
% Create branching example. E defines edges and V is a vertex array

n=75;
x=linspace(0,0.9*pi,n);
y=sin(x);
V=[x(:) y(:) zeros(size(x(:)))];
V=evenlySampleCurve(V,n,'pchip',0);
x=V(:,1);
y=V(:,2);
dV=vecnormalize(diff(V,1,1));
VdV=V(1:1:end-1,:);
placePoints=1:3:n-1;
E=[(1:1:n-1)' (2:1:n)'];
numSteps=numel(placePoints);
l=linspace(0.75,0.25,numSteps);
a=linspace(0.25*pi,0.25*pi,numSteps);
for q=1:1:numSteps
    R=euler2DCM([0 0 a(q)]);
    vr1=dV(placePoints(q),:)*R.*l(q);
    vr2=dV(placePoints(q),:)*R'.*l(q);
    Vb=linspacen([x(placePoints(q)) y(placePoints(q)) 0],vr1+[x(placePoints(q)) y(placePoints(q)) 0],n)';
    Eb=[(1:1:n-1)' (2:1:n)'];
    E=[E; Eb+size(V,1)];
    V=[V; Vb];
    Vb=linspacen([x(placePoints(q)) y(placePoints(q)) 0],vr2+[x(placePoints(q)) y(placePoints(q)) 0],n)';
    Eb=[(1:1:n-1)' (2:1:n)'];
    E=[E; Eb+size(V,1)];
    V=[V; Vb];
end
E=[E;E+size(V,1);];
V2=V;
V2(:,2)=-V2(:,2);
V2(:,1)=-V2(:,1);
V=[V;V2];
E=[E;E+size(V,1);];
V2=V;
V2(:,2)=-V2(:,2);
V=[V;V2];
[E,V]=mergeVertices(E,V);

%%
% Compute distances on mesh

%Option set
indStart=1; %Index of the start point
optionStruct.toleranceLevel=0; %Tolerance for convergence
optionStruct.numSeeds=1; %Number of seeds
optionStruct.waitBarOn=0; %Turn on/off waitbar

%Compute distances on mesh description
d=meshDistMarch(E,V,indStart,optionStruct);

%%
% Visualization

cFigure; hold on;
title('Distances on an edge / graph model','fontSize',fontSize);
hp(1)=gpatch(E,V,'none',d,1,4);
hp(2)=plotV(V(indStart,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Start point(s)'},'Location','SouthOutSide');
axisGeom;
view(2);
colorbar;
drawnow;

%% Example: Triangulated data

%%
% Get example triangulated mesh data
[F,V]=stanford_bunny;%graphicsModels(1);

%%
% Compute distances on mesh

%Option set
[~,indStart]=min(V(:,1)); %Index of the start point
optionStruct.toleranceLevel=0; %Tolerance for convergence
optionStruct.numSeeds=1; %Number of seeds
optionStruct.waitBarOn=0; %Turn on/off waitbar

%Compute distances on mesh description
d=meshDistMarch(F,V,indStart,optionStruct);

%%
% Visualization

cFigure;
subplot(1,2,1);hold on;
title('Distances on a triangulated surface model','fontSize',fontSize);
gpatch(F,V,'kw',d,1);
hp(1)=gpatch(F,V,'none',d,1,2);
hp(2)=plotV(V(indStart,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Start point(s)'},'Location','SouthOutSide');
axisGeom;
colorbar;
camlight headlight;

subplot(1,2,2);hold on;
title('Distances on a triangulated surface model','fontSize',fontSize);
hp(1)=gpatch(F,V,d,'none',1);
hp(2)=plotV(V(indStart,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Start point(s)'},'Location','SouthOutSide');
axisGeom;
camlight headlight;
colorbar;
drawnow;

%% Example: Quadrangulated data

%%
% Get example quadrangulated mesh data
n=4;
r=1;
[F,V]=quadSphere(n,r);

%%
% Compute distances on mesh

%Option set
[~,indStart]=min(V(:,1)); %Index of the start point
optionStruct.toleranceLevel=0; %Tolerance for convergence
optionStruct.numSeeds=1; %Number of seeds
optionStruct.waitBarOn=0; %Turn on/off waitbar

%Compute distances on mesh description
d=meshDistMarch(F,V,indStart,optionStruct);

%%
% Visualization

cFigure;
subplot(1,2,1);hold on;
title('Distances on a quad surface model','fontSize',fontSize);
gpatch(F,V,'kw',d,1);
hp(1)=gpatch(F,V,'none',d,1,2);
hp(2)=plotV(V(indStart,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Start point(s)'},'Location','SouthOutSide');
axisGeom;
colorbar;
camlight headlight;

subplot(1,2,2);hold on;
title('Distances on a quad surface model','fontSize',fontSize);
hp(1)=gpatch(F,V,d,'none',1);
hp(2)=plotV(V(indStart,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Start point(s)'},'Location','SouthOutSide');
axisGeom;
camlight headlight;
colorbar;
drawnow;

%% Example: Hexahedral mesh data

%%
% Get example hexahedral mesh data
cubeDimensions=[1 1 1];
cubeElementNumbers=[10 10 10];
outputStructType=2; %A structure compatible with mesh view
[meshStruct]=hexMeshBox(cubeDimensions,cubeElementNumbers,outputStructType);

%Access elements, nodes, and faces from the structure
HEX=meshStruct.elements; %The elements
F=meshStruct.faces; %The faces
V=meshStruct.nodes; %The nodes (vertices)

%Get edges for mesh marching
E=patchEdges(F); %Mesh edges

%%
% Compute distances on mesh

%Option set
[~,indStart]=min(V(:,1)); %Index of the start point
optionStruct.toleranceLevel=0; %Tolerance for convergence
optionStruct.numSeeds=1; %Number of seeds
optionStruct.waitBarOn=0; %Turn on/off waitbar

%Compute distances on mesh description
d=meshDistMarch(E,V,indStart,optionStruct);

%%
% Plotting model boundary surfaces and a cut view

cFigure; hold on
title('Distances on a hexahedral mesh','FontSize',fontSize);
hp(1)=gpatch(E,V,'none',d,1,3);
hp(2)=plotV(V(indStart,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Start point(s)'},'Location','SouthOutSide');
colorbar;
axisGeom(gca,fontSize);
drawnow;

%% Example: Tetrahedral mesh data

%%
% Get example tetrahedral mesh data
[TET,V]=hex2tet(HEX,V,[],2);
F=element2patch(TET,V); %Element faces

%Get edges for mesh marching
E=patchEdges(F); %Mesh edges

%%
% Compute distances on mesh

%Option set
[~,indStart]=min(abs(V(:,1)-min(V(:,1)))+abs(V(:,2)-max(V(:,2)))+abs(V(:,3)-min(V(:,3)))); %Index of the start point
optionStruct.toleranceLevel=0; %Tolerance for convergence
optionStruct.numSeeds=1; %Number of seeds
optionStruct.waitBarOn=0; %Turn on/off waitbar

%Compute distances on mesh description
d=meshDistMarch(E,V,indStart,optionStruct);

%%
% Plotting model boundary surfaces and a cut view

cFigure; hold on
title('Distances on a tetrahedral mesh','FontSize',fontSize);
hp(1)=gpatch(E,V,'none',d,1,3);
hp(2)=plotV(V(indStart,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Start point(s)'},'Location','SouthOutSide');
colorbar;
axisGeom(gca,fontSize);
drawnow;

%% Example: Mesh types effecting marching based distance computation
% In this example 3 mesh variations for a sphere are created. For each the
% start point is the point with the minimum x coordinate, i.e. a point on
% the far left of the plot. For each the true distance should be a smooth
% gradient with a maximum which equals pi.
% The marching algorithm uses the mesh to march and compute distances. One
% can imaging that straight paths (geodesically speaking) yeild the
% shortest distances. Hence zig-zag pattern (geodesically speaking) create
% false increased distances. This is not compensated for in this algorithm.
% Therefore the distance map depends on the mesh type and mesh
% connectivity. In the below example the regular quadrilateral mesh
% contains a straight path to the farthest point allong the equator. With
% mesh refinement the maximum distance would therefore converge on pi.
% However, other directions for the quadrilateral mesh produce zig-zag
% patterns causing distance to be altered. Hence the distance map departs
% from the smooth gradient allong the x-direction, one would expect. The
% triangulated mesh converted from the quadrilateral mesh contains both a
% straight path at the equator and also improved connectivity for other
% directions. The uniform geodesic triangulation contains the most smooth
% distance map.

%%
% Getting 3 different mesh types for a sphere.

n=3;
r=1;
[F1,V1]=quadSphere(n,r);
[F2,V2]=quad2tri(F1,V1);
[F3,V3]=geoSphere(n,r);

%%
% Compute distances on mesh

% -> Surface 1
%Option set
[~,indStart1]=min(V1(:,1)); %Index of the start point
optionStruct.toleranceLevel=0; %Tolerance for convergence
optionStruct.numSeeds=1; %Number of seeds
optionStruct.waitBarOn=0; %Turn on/off waitbar

%Compute distances on mesh description
[d1,i1]=meshDistMarch(F1,V1,indStart1,optionStruct);

% -> Surface 2
%Option set
[~,indStart2]=min(V2(:,1)); %Index of the start point
optionStruct.toleranceLevel=0; %Tolerance for convergence
optionStruct.numSeeds=1; %Number of seeds
optionStruct.waitBarOn=0; %Turn on/off waitbar

%Compute distances on mesh description
[d2,i2]=meshDistMarch(F2,V2,indStart2,optionStruct);

% -> Surface 3
%Option set
[~,indStart3]=min(V3(:,1)); %Index of the start point
optionStruct.toleranceLevel=0; %Tolerance for convergence
optionStruct.numSeeds=1; %Number of seeds
optionStruct.waitBarOn=0; %Turn on/off waitbar

%Compute distances on mesh description
[d3,i3]=meshDistMarch(F3,V3,indStart3,optionStruct);

%%
% Visualization

cFigure;
subplot(1,3,1);hold on;
title('Regular quad mesh','fontSize',fontSize);
hp(1)=gpatch(F1,V1,d1,d1);
hp(2)=plotV(V1(indStart1,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Start point(s)'},'Location','SouthOutSide');
axisGeom;
colorbar; caxis([0 pi]);
camlight headlight;
view(2);

subplot(1,3,2);hold on;
title('Irregular quad mesh','fontSize',fontSize);
hp(1)=gpatch(F2,V2,d2,d2);
hp(2)=plotV(V2(indStart2,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Start point(s)'},'Location','SouthOutSide');
axisGeom;
colorbar; caxis([0 pi]);
camlight headlight;
view(2);

subplot(1,3,3);hold on;
title('Geodesic triangulated mesh','fontSize',fontSize);
hp(1)=gpatch(F3,V3,d3,d3);
hp(2)=plotV(V3(indStart3,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Start point(s)'},'Location','SouthOutSide');
axisGeom;
colorbar; caxis([0 pi]);
camlight headlight;
view(2);

drawnow;

%% Example: Using |meshDistMarch| for geodesic point sampling

%%
% Get example triangulated mesh data
[F,V]=graphicsModels(7);

%%
% Compute distances on mesh

numSeeds=250;

%Option set
[~,indStart]=min(V(:,1)); %Index of the start point
optionStruct.toleranceLevel=0; %Tolerance for convergence
optionStruct.numSeeds=numSeeds; %Number of seeds
optionStruct.waitBarOn=1; %Turn on/off waitbar

%Use weigths based on z-direction
% W=V(:,3);
% W=W-min(W);
% W=W./max(W);
% W=(W*9)+1;
% optionStruct.W=W;

%Compute distances on mesh description
[d,seedIndex]=meshDistMarch(F,V,indStart,optionStruct);
[indSeeds,~,ind2]=unique(seedIndex);

%%
% Visualization

cFigure;
subplot(1,2,1); hold on;
title('Distances on a triangulated surface model','fontSize',fontSize);
hp(1)=gpatch(F,V,d,'none',1); hp(1).FaceColor='Interp';
hp(2)=plotV(V(indSeeds,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Seed point(s)'},'Location','SouthOutSide');
axisGeom;
camlight headlight;
colormap(gca,cMapDist); colorbar;

subplot(1,2,2); hold on;
title('Seed indices','fontSize',fontSize);
hp(1)=gpatch(F,V,'kw',ind2,1);
hp(2)=plotV(V(indSeeds,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Seed point(s)'},'Location','SouthOutSide');
axisGeom;
camlight headlight;
colormap(gca,cMapIndices); %icolorbar;
drawnow;

%% Example: Using |meshDistMarch| for geodesic surface resampling
% See also: |remeshTriSurfDistMap|

[Fd,Vd,indSeed]=seedIndex2triangulation(F,V,seedIndex);

%%
% Visualization

cFigure;
subplot(1,2,1); hold on;
title('Distances on a triangulated surface model','fontSize',fontSize);
hp(1)=gpatch(F,V,d,'none',1); hp(1).FaceColor='Interp';
hp(2)=plotV(V(indSeeds,:),'k.','MarkerSize',markerSize);
legend(hp,{'Mesh distances','Seed point(s)'},'Location','SouthOutSide');
axisGeom;
camlight headlight;
colormap(gca,cMapDist); colorbar;

subplot(1,2,2); hold on;
title('Resampled surface model','fontSize',fontSize);
plotV(V(indSeed,:),'k.','MarkerSize',50);
hp(1)=gpatch(F,V,'kw','none',0.5);
hp(2)=gpatch(Fd,Vd,'gw','k',1,2);
legend(hp,{'Original mesh','Resampled mesh'},'Location','SouthOutSide');

axisGeom;
camlight headlight;
drawnow;

%% Example: Using unit edge lengths
% Forcing unit edge lenghts means each edge is considered equally long i.e.
% a length of 1. Hence when used in combination with resampling, this
% causes the algorithm to resample dense regions in a dense fashion and
% coarse regions in a coarse fashion. Therefore using
% optionStruct.unitEdgeOn=1; one can force the resampling to have similar
% degrees of relative density differences. In the example below a dinosaur
% mesh with a fine mesh at the limbs and a coarse mesh on the main body is
% resampled using unit edge lengths. The output can be seen to remain
% refined at the limbs.

%%
% Get example triangulated mesh data
[F,V]=graphicsModels(4);
[F,V]=subtri(F,V,2);

%%
% Compute distances on mesh

%Option set
[~,indStart]=min(V(:,1)); %Index of the start point
optionStruct.toleranceLevel=0; %Tolerance for convergence
optionStruct.numSeeds=1000; %Number of seeds
optionStruct.waitBarOn=1; %Turn on/off waitbar
optionStruct.unitEdgeOn=1;

%Compute distances on mesh description
[d,seedIndex]=meshDistMarch(F,V,indStart,optionStruct);
[indSeeds,~,ind2]=unique(seedIndex);

%Compute resampled surface
[Fd,Vd,indSeed]=seedIndex2triangulation(F,V,seedIndex);

%%
% Visualization

cFigure;
subplot(1,2,1); hold on;
title('Distances on a triangulated surface model','fontSize',fontSize);
hp(1)=gpatch(F,V,d,'none',1); hp(1).FaceColor='Interp';
hp(2)=plotV(V(indSeeds,:),'k.','MarkerSize',25);
legend(hp,{'Mesh distances','Seed point(s)'},'Location','SouthOutSide');
axisGeom;
camlight headlight;
colormap(gca,cMapDist); colorbar;

subplot(1,2,2); hold on;
title('Resampled surface model','fontSize',fontSize);
plotV(V(indSeed,:),'k.','MarkerSize',25);
hp(1)=gpatch(F,V,'kw','none',0.5);
hp(2)=gpatch(Fd,Vd,'gw','k',1,2);
legend(hp,{'Original mesh','Resampled mesh'},'Location','SouthOutSide');

axisGeom;
camlight headlight;
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function filtered_image = gaussianbpf(I,d0,d1)
	% Butterworth Bandpass Filter
	% This simple  function was written for my Digital Image Processing course
	% at Eastern Mediterranean University taught by
	% Assoc. Prof. Dr. Hasan Demirel
	% for the 2010-2011 Spring Semester
	% for the complete report:
	% http://www.scribd.com/doc/51981950/HW4-Frequency-Domain-Bandpass-Filtering
	%
	% Written By:
	% Leonardo O. Iheme (leonardo.iheme@cc.emu.edu.tr)
	% 24th of March 2011
	%
	%   I = The input grey scale image
	%   d0 = Lower cut off frequency
	%   d1 = Higher cut off frequency
	%
	% The function makes use of the simple principle that a bandpass filter
	% can be obtained by multiplying a lowpass filter with a highpass filter
	% where the lowpass filter has a higher cut off frquency than the high pass filter.
	%
	% Usage GAUSSIANBPF(I,DO,D1)
	% Example
	% ima = imread('grass.jpg');
	% ima = rgb2gray(ima);
	% filtered_image = gaussianbpf(ima,30,120);
	% Gaussian Bandpass Filter
	%
	% biafra ahanonu
	% updated: 2013.11.09 [13:20:43]
	% changelog
		% 2013.11.09 [13:21:06] updated function so it no longer converts to uint8 as this causes problems. Also, changed filtered_image = fftI + filter3.*fftI; to filtered_image = filter3.*fftI;
		% 2014.06.03 - updated to do binary fft

	f = double(I);
	[nx ny] = size(f);
	% f = uint8(f);
	fftI = fft2(f,2*nx-1,2*ny-1);
	fftI = fftshift(fftI);

	% subplot(2,2,1)
	% imshow(f,[]);
	% title('Original Image')

	% subplot(2,2,2)
	% fftshow(fftI,'log')
	% title('Fourier Spectrum of Image')
	% Initialize filter.
	% filter1 = ones(2*nx-1,2*ny-1);
	% filter2 = ones(2*nx-1,2*ny-1);
	% filter3 = ones(2*nx-1,2*ny-1);
	% % ======
	% % Gaussian
	% for i = 1:2*nx-1
	%     for j =1:2*ny-1
	%         dist = ((i-(nx+1))^2 + (j-(ny+1))^2)^.5;
	%         % Use Gaussian filter.
	%         filter1(i,j) = exp(-dist^2/(2*d1^2));
	%         filter2(i,j) = exp(-dist^2/(2*d0^2));
	%         filter3(i,j) = 1.0 - filter2(i,j);
	%         filter3(i,j) = filter1(i,j).*filter3(i,j);
	%     end
	% end
	% ======
	% binary
	imageSize = size(fftI);
	ci = [round(imageSize(1)/2), round(imageSize(2)/2), 10];     % center and radius of circle ([c_row, c_col, r])
	[xx,yy] = ndgrid((1:imageSize(1))-ci(1),(1:imageSize(2))-ci(2));
	filter3 = logical((xx.^2 + yy.^2)<(ci(3)^2));
	% imagesc(mask)
	% ======
	% Update image with passed frequencies
	fftI_filtered = filter3.*fftI;
	% subplot(2,2,3)
	% fftshow(filter3,'log')
	% title('Frequency Domain Filter Function Image')
	filtered_image = ifftshift(fftI_filtered);
	filtered_image = ifft2(filtered_image,2*nx-1,2*ny-1);
	filtered_image = real(filtered_image(1:nx,1:ny));
	filtered_image = double(filtered_image);
	% filtered_image = uint8(filtered_image);
	% ======
	% imagesc(filter3)
	subplot(2,2,1)
	fftshow(fftI_filtered,'log')
	subplot(2,2,2)
	fftshow(fftI,'log')
	subplot(2,2,3)
	imagesc(I)
	subplot(2,2,4)
	imshow(filtered_image,[])
	title('Bandpass Filtered Image')


function [ i_lo, i_hi ] = r8vec_sorted_range ( n, r, r_lo, r_hi )

%*****************************************************************************80
%
%% R8VEC_SORTED_RANGE searches a sorted vector for elements in a range.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of items in the vector.
%
%    Input, real R(N), the sorted vector.
%
%    Input, real R_LO, R_HI, the limits of the range.
%
%    Output, integer I_LO, I_HI, the range of indices
%    so that I_LO <= I <= I_HI => R_LO <= R(I) <= R_HI.  If no
%    values in R lie in the range, then I_HI < I_LO will be returned.
%

%
%  Cases we can handle immediately.
%
  if ( r(n) < r_lo )
    i_lo = 0;
    i_hi = - 1;
    return
  end

  if ( r_hi < r(1) )
    i_lo = 0;
    i_hi = - 1;
    return
  end
%
%  Are there are least two intervals?
%
  if ( n == 1 )
    if ( r_lo <= r(1) && r(1) <= r_hi )
      i_lo = 1;
      i_hi = 1;
    else
      i_lo = 0;
      i_hi = -1;
    end
    return
  end
%
%  Bracket R_LO.
%
  if ( r_lo <= r(1) )

    i_lo = 1;

  else
%
%  R_LO is in one of the intervals spanned by R(J1) to R(J2).
%  Examine the intermediate interval [R(I1), R(I1+1)].
%  Does R_LO lie here, or below or above?
%
    j1 = 1;
    j2 = n;
    i1 = floor ( ( j1 + j2 - 1 ) / 2 );
    i2 = i1 + 1;

    while ( 1 )

      if ( r_lo < r(i1) )
        j2 = i1;
        i1 = floor ( ( j1 + j2 - 1 ) / 2 );
        i2 = i1 + 1;
      elseif ( r(i2) < r_lo )
        j1 = i2;
        i1 = floor ( ( j1 + j2 - 1 ) / 2 );
        i2 = i1 + 1;
      else
        i_lo = i1;
        break;
      end

    end

  end
%
%  Bracket R_HI
%
  if ( r(n) <= r_hi )

    i_hi = n;

  else

    j1 = i_lo;
    j2 = n;
    i1 = floor ( ( j1 + j2 - 1 ) / 2 );
    i2 = i1 + 1;

    while ( 1 )

      if ( r_hi < r(i1) )
        j2 = i1;
        i1 = floor ( ( j1 + j2 - 1 ) / 2 );
        i2 = i1 + 1;
      elseif ( r(i2) < r_hi )
        j1 = i2;
        i1 = floor ( ( j1 + j2 - 1 ) / 2 );
        i2 = i1 + 1;
      else
        i_hi = i2;
        break
      end

    end

  end
%
%  We expect to have computed the largest I_LO and smallest I_HI such that
%    R(I_LO) <= R_LO <= R_HI <= R(I_HI)
%  but what we want is actually
%    R_LO <= R(I_LO) <= R(I_HI) <= R_HI
%  which we can usually get simply by incrementing I_LO and decrementing I_HI.
%
  if ( r(i_lo) < r_lo )
    i_lo = i_lo + 1;
    if ( n < i_lo )
      i_hi = i_lo - 1;
    end
  end

  if ( r_hi < r(i_hi) )
    i_hi = i_hi - 1;
    if ( i_hi < 1 )
      i_lo = i_hi + 1;
    end
  end

  return
end


close all
clear

pos_rob = [-0.5,-0.5;0.5,0.5];
pos_ref =  [1.1,1;-1.1,-1];
pos_circ = [-0.5,-0.5;0.5,0.5]/2;

% circular constraint sets
zbf_circ = @(i,pos_rob) 1-(pos_rob(i,:)-pos_circ(i,:))*(pos_rob(i,:)-pos_circ(i,:))';
% nominal control
knom = 1;
unom_rob = @(i,pos_rob) -knom*(pos_rob(i,:)-pos_ref(i,:));
% bounded distance costraints
ld = 0.5; ud = 1;
zbf_lb = @(i,j,pos_rob) (pos_rob(i,:)-pos_rob(j,:))*(pos_rob(i,:)-pos_rob(j,:))'-ld^2;
zbf_ub = @(i,j,pos_rob) ud^2-(pos_rob(i,:)-pos_rob(j,:))*(pos_rob(i,:)-pos_rob(j,:))';
% extended K-class function
akcf = 1;
ekcf = @(h) akcf*h;
% control barrier function (less or equal)
cbfa_circ = @(i,pos_rob) pos_rob(i,:)-pos_circ(i,:);
cbfb_circ = @(i,pos_rob) ekcf(zbf_circ(i,pos_rob));
cbfa_lb = @(i,j,pos_rob) -(pos_rob(i,:)-pos_rob(j,:));
cbfb_lb = @(i,j,pos_rob) ekcf(zbf_lb(i,j,pos_rob));
cbfa_ub = @(i,j,pos_rob) -cbfa_lb(i,j,pos_rob);
cbfb_ub = @(i,j,pos_rob) ekcf(zbf_ub(i,j,pos_rob));

% simulation
dt = 0.01;
T = 10;
loop = 0;
color_list = ['b','g','m','r','c'];
for t=0:dt:T
    loop = loop+1;
    % control barrier function - quadratic program
    for i=1:2
        fqp = -unom_rob(i,pos_rob)';
        aqp = cbfa_circ(i,pos_rob);
        bqp = cbfb_circ(i,pos_rob);
        for j=1:2
            if i~=j
                aqp = [aqp;cbfa_lb(i,j,pos_rob)];
                bqp = [bqp;cbfb_lb(i,j,pos_rob)];
                aqp = [aqp;cbfa_ub(i,j,pos_rob)];
                bqp = [bqp;cbfb_ub(i,j,pos_rob)];
            end
        end
        [uqp,~,is_solved] = quadprog(eye(2),fqp,aqp,bqp);
        uqp_rob(i,:) = uqp';
    end
    % update
    pos_rob = pos_rob+dt*uqp_rob;
    % plot
    for i=1:2
        plot(pos_rob(i,1),pos_rob(i,2),[color_list(i) 'o']); hold on
        circle_(pos_circ(i,:),1,'color',color_list(i));
        plot(pos_ref(i,1),pos_ref(i,2),[color_list(i) 'd']);
        quiver(pos_rob(i,1),pos_rob(i,2),uqp_rob(i,1),uqp_rob(i,2))
    end
    hold off
    drawnow
end


function y = nanmax(x,dim)
% nanmax - Max ignoring NaNs.
%
% Synopsis:
%   y = nanmax(x)
%   y = nanmax(x,dim)
%
% Arguments:
%   x: Matrix or vector
%   dim: Dimension along which sum operates. Default: First non-singleton
%       dimension.
%
% Returns:
%   y: max along the chosen dimension, treating NaNs as missing values.
%
% Description:
%   For vectors, nanmax(X) is the max of the non-NaN elements in
%   X. For matrices, nanmax(X) is a row vector containing the max
%   of the non-NaN elements in each column of X. For N-D arrays,
%   nanmax(X) operates along the first non-singleton dimension.
%
%   nanmax(X,dim) determaxes the max along the dimension dim.
%
% Examples:
%   nanmax([1 2 NaN]) returns 2.
%   nanmax([1 2 NaN], 1) returns [1 2 NaN].
%   nanmax([1 2 NaN], 2) returns 2.
%
% See also: nansum,nanmean,nanstd
%

% Author(s): Benjamax Blankertz, Anton Schwaighofer, Aug 2005

if nargin<2,
  % Operate along the first non-singleton dimension
  dim = max(find(size(x)~=1));
  if isempty(dim),
    dim = 1;
  end
end
% Replace NaNs with zeros.
nans = isnan(x);
x(nans) = -inf;

% Protect against an entire column of NaNs
y = max(x, [], dim);
allNaNs = all(nans, dim);
y(allNaNs) = NaN;


%%*************************************************************************
%% iterrefine: Iterative refinement.
%% This step is crucial to ensure that computed solution
%% is sufficiently accuraete.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*************************************************************************

   function [x,resnrm,solve_ok] = iterrefine(A,b,L,x0);

     tol = 1e-6;
     maxit = 10;
     bnorm = max(1,norm(b));
     tolb = tol*bnorm;
     x = x0;
     solve_ok = 1;
     resnrm(1) = bnorm;
%%
     for iter = 1:maxit
        x0 = x;
        if isstruct(A); r = b-matvec(A,x); else; r=b-A*x; end;
        err = norm(r);
        resnrm(iter+1) = err;

        if (err < tolb); break; end;
        if (iter > 1) & (resnrm(iter+1)/resnrm(iter) > 0.9)
           x = x0; solve_ok = 0; break;
        end
	d = linsysolvefun(L,r);
        x = x + d;
     end
%%*************************************************************************
%% matvec: matrix-vector multiply.
%% matrix = [A.mat11 A.mat12; A.mat12' A.mat22]
%%*************************************************************************

   function Ax = matvec(A,x);

   m = length(A.mat11); m2 = length(x)-m;
   x1 = x(1:m);
   Ax = A.mat11*x1;
   if (m2 > 0)
      x2 = x(m+[1:m2]);
      Ax = Ax + A.mat12*x2;
      Ax2 = (x1'*A.mat12)' + A.mat22*x2;
      Ax = [Ax; Ax2];
   end
   return;
%%*************************************************************************


function [ a, ipvt, info ] = dchdc ( a, lda, p, ipvt, job )

%*****************************************************************************80
%
%% DCHDC computes the Cholesky decomposition of a positive definite matrix.
%
%  Discussion:
%
%    A pivoting option allows the user to estimate the condition of a
%    positive definite matrix or determine the rank of a positive
%    semidefinite matrix.
%
%    For positive definite matrices, INFO = P is the normal return.
%
%    For pivoting with positive semidefinite matrices, INFO will
%    in general be less than P.  However, INFO may be greater than
%    the rank of A, since rounding error can cause an otherwise zero
%    element to be positive.  Indefinite systems will always cause
%    INFO to be less than P.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,P), the matrix whose decomposition is to
%    be computed.  Only the upper half of A need be stored.
%    The lower part of the array a is not referenced.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer P, the order of the matrix.
%
%    Input, integer IPVT(P), integers that control the selection
%    of the pivot elements, if pivoting has been requested.
%    Each diagonal element A(K,K) is placed in one of three classes
%    according to the value of IPVT(K).
%
%      > 0, then X(K) is an initial element.
%      = 0, then X(K) is a free element.
%      < 0, then X(K) is a final element.
%
%    Before the decomposition is computed, initial elements are moved by
%    symmetric row and column interchanges to the beginning of the array A
%    and final elements to the end.  Both initial and final elements are
%    frozen in place during the computation and only free elements are moved.
%    At the K-th stage of the reduction, if A(K,K) is occupied by a free
%    element, it is interchanged with the largest free element A(L,L) with
%    K <= L.  IPVT is not referenced if JOB is 0.
%
%    Input, integer JOB, initiates column pivoting.
%    0, no pivoting is done.
%    nonzero, pivoting is done.
%
%    Output, real A(LDA,P), contains in its upper half the Cholesky factor
%    of the input matrix, as it has been permuted by pivoting.
%
%    Output, integer IPVT(N); IPVT(J) contains the index of the diagonal element
%    of A that was moved into the J-th position, if pivoting was requested.
%
%    Output, integer INFO, contains the index of the last positive diagonal
%    element of the Cholesky factor.
%
  pl = 1;
  pu = 0;
  info = p;

  if ( job ~= 0 )
%
%  Pivoting has been requested.
%  Rearrange the the elements according to IPVT.
%
    for k = 1 : p

      swapk = 0 < ipvt(k);

      negk = ipvt(k) < 0;

      if ( negk )
        ipvt(k) = -k;
      else
        ipvt(k) = k;
      end

      if ( swapk )

        if ( k ~= pl )

          temp(1:pl-1) = a(1:pl-1,k);
          a(1:pl-1,k) = a(1:pl-1,pl);
          a(1:pl-1,pl) = temp(1:pl-1);

          temp = a(k,k);
          a(k,k) = a(pl,pl);
          a(pl,pl) = temp;

          for j = pl+1 : p

            if ( j < k )
              temp = a(pl,j);
              a(pl,j) = a(j,k);
              a(j,k) = temp;
            elseif ( k < j )
              temp = a(k,j);
              a(k,j) = a(pl,j);
              a(pl,j) = temp;
            end

          end

          ipvt(k) = ipvt(pl);
          ipvt(pl) = k;

        end

        pl = pl + 1;

      end

    end

    pu = p;

    for k = p : -1 : pl

      if ( ipvt(k) < 0 )

        ipvt(k) = -ipvt(k);

        if ( pu ~= k )

          temp(1:k-1) = a(1:k-1,k);
          a(1:k-1,k) = a(1:k-1,pu);
          a(1:k-1,pu) = temp(1:k-1);

          temp = a(k,k);
          a(k,k) = a(pu,pu);
          a(pu,pu) = temp;

          for j = k+1 : p

            if ( j < pu )
              temp = a(k,j);
              a(k,j) = a(j,pu);
              a(j,pu) = temp;
            elseif ( pu < j )
              temp = a(k,j);
              a(k,j) = a(pu,j);
              a(pu,j) = temp;
            end

          end

          jt = ipvt(k);
          ipvt(k) = ipvt(pu);
          ipvt(pu) = jt;

        end

        pu = pu - 1;

      end

    end

  end

  for k = 1 : p
%
%  Reduction loop.
%
    maxdia = a(k,k);
    maxl = k;
%
%  Determine the pivot element.
%
    if ( pl <= k & k < pu )

      for l = k+1 : pu
        if ( maxdia < a(l,l) )
          maxdia = a(l,l);
          maxl = l;
        end
      end

    end
%
%  Quit if the pivot element is not positive.
%
    if ( maxdia <= 0.0 )
      info = k - 1;
      return
    end
%
%  Start the pivoting and update IPVT.
%
    if ( k ~= maxl )

      temp(1:k-1) = a(1:k-1,k);
      a(1:k-1,k) = a(1:k-1,maxl);
      a(1:k-1,maxl) = temp(1:k-1);

      a(maxl,maxl) = a(k,k);
      a(k,k) = maxdia;
      jp = ipvt(maxl);
      ipvt(maxl) = ipvt(k);
      ipvt(k) = jp;

    end
%
%  Reduction step.
%  Pivoting is contained across the rows.
%
    work(k) = sqrt ( a(k,k) );
    a(k,k) = work(k);

    for j = k+1 : p

      if ( k ~= maxl )

        if ( j < maxl )
          temp = a(k,j);
          a(k,j) = a(j,maxl);
          a(j,maxl) = temp;
        elseif ( maxl < j )
          temp = a(k,j);
          a(k,j) = a(maxl,j);
          a(maxl,j) = temp;
        end

      end

      a(k,j) = a(k,j) / work(k);
      work(j) = a(k,j);
      temp = -a(k,j);
      a(k+1:k+j-k,j) = a(k+1:k+j-k,j) + temp * work(k+1:k+j-k)';

    end

  end

  return
end


function[varargout]=windtrans(varargin)
%WINDTRANS  Ekman-like transfer-functions for the wind-driven response.
%
%   G=WINDTRANS(OMEGA,Z,FC,DELTA,MU,H) returns the no-slip transfer
%   function for the wind-driven currents evaluated at frequencies OMEGA
%   and at depths Z.  G will have LENGTH(OMEGA) rows and LENGTH(Z) columns.
%
%   Here FC is the local Coriolis frequency, DELTA is the Ekman depth.
%   MU is the Madsen depth, and H is the boundary layer depth.
%
%   The units of these quantitites are important.  For consistency with
%   other routines, OMEGA and FC are in radians per day, while Z, DELTA,
%   MU, and H are all in meters.  The units of G are then m^2 s / kg.
%
%   WINDTRANS(...,'free') instead returns the free slip transfer function.
%
%   For details on the expressions implemented by this function, see
%
%        Lilly, J. M. and S. Elipot (2021). A unifying perspective on
%            transfer function solutions to the unsteady Ekman problem.
%            Fluids, 6 (2): 85, 1--36.
%   __________________________________________________________________
%
%   Special forms
%
%   By default WINDTRANS uses the general transfer function.  WINDTRANS
%   will also employ limiting expressions for special cases, as follows.
%
%       H = Inf                --  Mixed Ekman / Madsen solution
%       H = Inf, MU = 0        --  Ekman solution
%       H = Inf, DELTA = 0     --  Madsen solution
%       MU = 0                 --  Finite-layer Ekman solution
%       DELTA = 0              --  Finite-layer Madsen solution
%
%   These have to be coded separately because the full solution is singular
%   in these cases.
%   __________________________________________________________________
%
%   Computational options
%
%   When computed over a wide range of parameter space, the transfer
%   function tends to be encounter numerical overflow when the arguments to
%   Bessel functions become large, causing its computation to fail.
%
%   To avoid this problems, by default WINDTRANS switches to using a highly
%   accurate thirty-term expansion about the large-argument exponential
%   behavior of the Bessel functions when their arguments exceed 10^2.9.
%
%   Two other options are available, primarily for testing purposes. Both
%   of these other algorithms lead to artifacts and are not recommended.
%
%   WINDTRANS(...'far',...) switches instead to use the (inferior) one-term
%   expansion, also known as the far-inertial limit.
%
%   WINDTRANS(...,'general',...) uses the general formula with no switch.
%
%   Note that these options only apply to no-slip solution.  The free-slip
%   solution, which is not deemed to be physically relevant, is only
%   computed with the general formula.
%
%   For details on these algorithms, see Lilly and Elipot (2021).
%   __________________________________________________________________
%
%   'windtrans --t' runs a some tests.
%
%   Usage: G=windtrans(omega,z,fc,delta,mu,h);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2019--2021 J.M. Lilly --- type 'help jlab_license' for details

%   WINDTRANS(OMEGA,Z,FC,DELTA,MU,A,'AMP') alternately parameterizes the
%   transfer function in terms of the magntide of the near-inertial peak A
%   in 1/m, together with the Ekman and Madsen depths DELTA and MU.

%   Gradient output
%
%   [G,DDELTA,DH]=WINDTRANS(OMEGA,Z,FC,DELTA,0,H) for the Ekman-like model
%   also returns the partial derivatives with respect to DELTA and H.

if strcmp(varargin{1}, '--t')
    windtrans_test,return
end

omega=varargin{1}(:)/24/3600;%convert to rad/second
z=varargin{2};
fc=varargin{3}/24/3600;%convert to rad/second
delta=varargin{4};
mu=varargin{5};
h=varargin{6};

tol=100;
rho=1027;
str='expansion';%use the 30-term tilde expansion by default
%str='two';%use the two-term tilde expansion by default
slipstr='noslip';

bool=false(2,1);
for i=7:nargin
    istr=varargin{i}(1:3);
    if strcmpi(istr,'gen')||strcmpi(istr,'eli')||strcmpi(istr,'far')||strcmpi(istr,'ada')||strcmpi(istr,'exp')||strcmpi(istr,'two')
        str=varargin{i};
        if strcmpi(str(1:3),'exp')
            bool(1)=true;
        end
    elseif strcmpi(istr,'fre')||strcmpi(istr,'nos')
        slipstr=varargin{i};
        if strcmpi(slipstr(1:3),'fre')
            bool(2)=true;
        end
    end
end

%this is just to keep track of an unsuitable combination of input arguments
if all(bool)
     disp('Sorry, the tilde expansion algorithm is not implemented for the free-slip transfer function.')
end
%these may have been either input, or reflecting the default behavior
if strcmpi(str(1:3),'exp')&&strcmpi(slipstr(1:3),'fre')
     str='gen';
end

omega1=omega;
if length(z)~=1
    [z,omega]=meshgrid(z,omega);
end

%if isinf(h)||delta==0||mu==0
%    str='lilly';%use the lilly version of expressions for special cases
%end

%str,slipstr
G=zeros(size(omega));
if strcmpi(slipstr(1:3),'nos')
    %----------------------------------------------------------------------
    %The noslip forms
        %[G,xiz,xih,xi0]=windtrans_general_noslip(delta,fc,rho,z,mu,omega,h,str);
   %     G=windtrans_general_noslip(delta,fc,rho,z,mu,omega,h,str);
    if strcmpi(str(1:3),'gen')|| strcmpi(str(1:3),'far')||strcmpi(str(1:3),'exp')||strcmpi(str(1:3),'two')
        [G,varargout{2},varargout{3}]=windtrans_lilly_noslip(delta,fc,rho,z,mu,omega,h,str);
    elseif strcmpi(str(1:3),'eli')%asymptotic forms, elipot edition
        G=windtrans_elipot_noslip(delta,fc,rho,z,mu,omega,h);
    end
elseif strcmpi(slipstr(1:3),'fre')
    %----------------------------------------------------------------------
    %The freeslip forms

    K0=frac(1,2)*delta.^2.*abs(fc);
    K1=frac(1,2)*mu*abs(fc);

    s=sign(fc).*sign(1+omega./fc);
    zo=delta.^2./mu;
    [xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h);

    coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*rho.*abs(fc).*sqrt(abs(1+omega./fc)));
    [k0z,i0z,k1h,i1h,k10,i10]=bessels_freeslip(xiz,xih,xi0);

    if strcmpi(str(1:3),'gen')||(strcmpi(str(1:3),'lil')&&((K0~=0)&&(K1~=0)))
        numer=i0z.*k1h+i1h.*k0z;
        denom=i1h.*k10-i10.*k1h;
        G=coeff.*frac(numer,denom);
    elseif strcmpi(str(1:3),'lil')
        %    [h,K0,K1]
        if mu==0
            coeff=frac(sqrt(2).*rot(-s*pi/4),delta.*abs(fc).*rho.*sqrt(abs((1+omega./fc))));
            cosharg=sqrt(2).*rot(s*pi/4).*frac(h-z,delta).*sqrt(abs((1+omega./fc)));
            sinharg=sqrt(2).*rot(s*pi/4).*frac(h,delta).*sqrt(abs((1+omega./fc)));
            G=coeff.*frac(cosh(cosharg),sinh(sinharg));
        elseif delta==0
            coeff=frac(2,rho*K1);
            k0z=besselk(0,2*rot(s*pi/4).*sqrt(frac(z,K1./abs(fc)).*abs(1+omega./fc)));
            k1h=besselk(1,2*rot(s*pi/4).*sqrt(frac(h,K1./abs(fc)).*abs(1+omega./fc)));
            i0z=besseli(0,2*rot(s*pi/4).*sqrt(frac(z,K1./abs(fc)).*abs(1+omega./fc)));
            i1h=besseli(1,2*rot(s*pi/4).*sqrt(frac(h,K1./abs(fc)).*abs(1+omega./fc)));
            G=coeff.*(k0z+frac(k1h.*i0z,i1h));
        end
    elseif strcmpi(str(1:3),'eli')

        delta1=sqrt(2.*K0./(omega+fc));
        delta2=K1./(omega+fc);
        xize=2*sqrt(1i*(zo+z)./delta2);
        xi0e=2*sqrt(1i*zo./delta2);
        xihe=2*sqrt(1i*(zo+h)./delta2);

        if mu==0
            coeff=frac(1,rho.*sqrt(1i*(omega+fc).*K0));
            numer=cosh((1+1i).*(h-z)./delta1);
            denom=sinh((1+1i).*h./delta1);
            G=coeff.*numer./denom;
        elseif delta==0
            coeff=(2./rho./K1);
            k0z=besselk(0,2.*sqrt(1i.*z./delta2));
            k1h=besselk(1,2*sqrt(1i.*h./delta2));
            i0z=besseli(0,2*sqrt(1i.*z./delta2));
            i1h=besseli(1,2*sqrt(1i.*h./delta2));
            G=coeff.*(k0z+frac(k1h.*i0z,i1h));
        else
            coeff=frac(1,rho.*sqrt(1i.*(omega+fc).*K0));
            k0z=besselk(0,xize);
            k10=besselk(1,xi0e);
            i10=besseli(1,xi0e);
            i0z=besseli(0,xize);
            k1h=besselk(1,xihe);
            i1h=besseli(1,xihe);
            numer=i0z.*k1h+i1h.*k0z;
            denom=i1h.*k10-i10.*k1h;
            G=coeff.*frac(numer,denom);
        end
    end
end
G(z>h)=nan;

varargout{1}=G;
%varargout{2}=xih;
%varargout{3}=xiz;
%varargout{4}=xi0;

function[xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h)

xiz=2*sqrt(2).*rot(s.*pi/4).*(zo./delta).*sqrt((1+z./zo).*abs((1+omega./fc)));
xih=2*sqrt(2).*rot(s.*pi/4).*(zo./delta).*sqrt((1+h./zo).*abs((1+omega./fc)));
xi0=2*sqrt(2).*rot(s.*pi/4).*(zo./delta).*sqrt(abs((1+omega./fc)));

function[k0z,i0z,k0h,i0h,k10,i10]=bessels_noslip(varargin)

argz=varargin{1};
argh=varargin{2};
if length(varargin)==3
    arg0=varargin{3};
end

k0z=besselk(0,argz);
i0z=besseli(0,argz);
k0h=besselk(0,argh);
i0h=besseli(0,argh);

if nargout>4
    k10=besselk(1,arg0);
    i10=besseli(1,arg0);
end

function[k0z,i0z,k0h,i0h,k10,i10]=besseltildes_noslip(argz,argh,arg0,nterms)

k0z=besselktilde(0,argz,nterms);
i0z=besselitilde(0,argz,nterms);
k0h=besselktilde(0,argh,nterms);
i0h=besselitilde(0,argh,nterms);

k10=besselktilde(1,arg0,nterms);
i10=besselitilde(1,arg0,nterms);

function[k0z,i0z,k1h,i1h,k10,i10]=bessels_freeslip(argz,argh,arg0)

k0z=besselk(0,argz);
i0z=besseli(0,argz);
k1h=besselk(1,argh);
i1h=besseli(1,argh);

if nargout>4
    k10=besselk(1,arg0);
    i10=besseli(1,arg0);
end

function[G]=windtrans_expansion_noslip(delta,fc,rho,z,mu,omega,h,str)

zo=delta.^2./mu;
s=sign(fc).*sign(1+omega./fc);
[xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h);
coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*rho.*abs(fc).*sqrt(abs(1+omega./fc)));

if strcmpi(str(1:3),'two')
    [k0z,i0z,k0h,i0h,k10,i10]=besseltildes_noslip(xiz,xih,xi0,2);
elseif strcmpi(str(1:3),'exp')
    [k0z,i0z,k0h,i0h,k10,i10]=besseltildes_noslip(xiz,xih,xi0,30);
end

numer=exp(xi0-xiz).*i0h.*k0z-exp(xi0+xiz-2*xih).*k0h.*i0z;
denom=i0h.*k10+exp(2*xi0-2*xih).*k0h.*i10;
G=coeff.*frac(numer,denom);

bool=(omega==-fc);
if ~isinf(h)&&~isinf(zo)
    if length(z)==1
        G(bool)=frac(4*zo,rho*abs(fc)*delta.^2).*frac(sqrt(1+h./zo)-sqrt(1+z./zo),(1+z./zo).^(1/4));
    else
        G(bool)=frac(4*zo,rho*abs(fc)*delta.^2).*frac(sqrt(1+h./zo)-sqrt(1+z(bool)./zo),(1+z(bool)./zo).^(1/4));
    end
elseif isinf(h)
    G(bool)=inf;
end

function[G,xiz,xih,xi0]=windtrans_general_noslip(delta,fc,rho,z,mu,omega,h,str)

zo=delta.^2./mu;
s=sign(fc).*sign(1+omega./fc);
[xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h);
coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*rho.*abs(fc).*sqrt(abs(1+omega./fc)));

[k0z,i0z,k0h,i0h,k10,i10]=bessels_noslip(xiz,xih,xi0);

%numerically, better to do it like this to avoid having huge numbers dominate
%tests shows this removes the large-argument overflow when only h (not z) is large

numer=k0z./k10-(k0h./k10).*(i0z./i0h);
denom=1+(k0h./k10).*(i10./i0h);
G=coeff.*frac(numer,denom);

if strcmpi(str(1:3),'exp')||strcmpi(str(1:3),'two')
    bool=log10(abs(xiz))>2.9;
    if ~isempty(find(bool,1))
        if length(z)==1
            G(bool)=windtrans_expansion_noslip(delta,fc,rho,z,mu,omega(bool),h,str);
        else
            G(bool)=windtrans_expansion_noslip(delta,fc,rho,z(bool),mu,omega(bool),h,str);
        end
    end
elseif strcmpi(str(1:3),'far')
    bool=log10(abs(xiz))>2.9;
    if ~isempty(find(bool,1))
        if length(z)==1
            G(bool)=windtrans_farinertial_noslip(delta,fc,rho,z,mu,omega(bool),h);
        else
            G(bool)=windtrans_farinertial_noslip(delta,fc,rho,z(bool),mu,omega(bool),h);
        end
    end
end
G=windtrans_inertiallimit(G,delta,fc,rho,z,mu,omega,h);

function[G]=windtrans_farinertial_noslip(delta,fc,rho,z,mu,omega,h)

zo=delta.^2./mu;
s=sign(fc).*sign(1+omega./fc);
[xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h);
coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*abs(fc).*rho);

numer=exp(-xiz+xi0)-exp(xiz+xi0-2*xih);
denom=1+exp(2*xi0-2*xih);
G=coeff.*frac(1,sqrt(abs(1+omega./fc)).*(1+z./zo).^(1/4)).*frac(numer,denom);
%Don't apply the inertial limit!

function[G]=windtrans_inertiallimit(G,delta,fc,rho,z,mu,omega,h)

zo=delta.^2./mu;
bool=(omega==-fc);

if ~isempty(find(bool,1))
    if ~isinf(h)&&~isinf(zo)
        if length(z)==1
            %G(bool)=frac(2*zo,rho*abs(fc)*delta.^2).*log(frac(1+h./zo,1+z./zo));
            G(bool)=frac(2,rho*abs(fc)*mu).*log(frac(1+h./zo,1+z./zo));
        else
            %G(bool)=frac(2*zo,rho*abs(fc)*delta.^2).*log(frac(1+h./zo,1+z./zo));
            G(bool)=frac(2,rho*abs(fc)*mu).*log(frac(1+h./zo,1+z(bool)./zo));
        end
    elseif ~isinf(h)&&isinf(zo)
        if length(z)==1
            G(bool)=frac(2,rho*abs(fc)*delta.^2).*(h-z);
        else
            G(bool)=frac(2,rho*abs(fc)*delta.^2).*(h-z(bool));
        end
    else
        G(bool)=inf;
    end
end

function[G,ddelta,dh]=windtrans_lilly_noslip(delta,fc,rho,z,mu,omega,h,str)

zo=delta.^2./mu;
s=sign(fc).*sign(1+omega./fc);
[xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h);
%[delta,zo,K0,K1]

if h==inf
    if mu==0
        %Ekman solution
        coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*abs(fc).*rho);
        G=coeff.*frac(exp(-(1+s.*1i).*(z./delta).*sqrt(abs((1+omega./fc)))),sqrt(abs((1+omega./fc))));
    elseif delta==0
        %Madsen solution
        coeff=frac(4,rho*abs(fc)*mu);
        G=coeff.*besselk(0,2*sqrt(2)*rot(s.*pi/4).*sqrt(frac(z,mu).*abs(1+omega./fc)));
    else
        %Mixed solution
        k0z=besselk(0,xiz);
        k10=besselk(1,xi0);
        coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*abs(fc).*rho.*sqrt(abs((1+omega./fc))));
        G=coeff.*frac(k0z,k10);
    end
else
    if mu==0
        %finite-layer Ekman
        coeff=frac(sqrt(2).*rot(-s*pi/4),delta.*abs(fc).*rho.*sqrt(abs((1+omega./fc))));
        %sinharg=sqrt(2).*rot(s*pi/4).*frac(h-z,delta).*sqrt(abs((1+omega./fc)));
        %cosharg=sqrt(2).*rot(s*pi/4).*frac(h,delta).*sqrt(abs((1+omega./fc)));
        %G=coeff.*frac(sinh(sinharg),cosh(cosharg));
        %can't do it this way because you divide two huge numbers

        argh=sqrt(2).*rot(s*pi/4).*frac(h,delta).*sqrt(abs((1+omega./fc)));
        argz=sqrt(2).*rot(s*pi/4).*frac(z,delta).*sqrt(abs((1+omega./fc)));

        numer=exp(-argz)-exp(argz).*exp(-2*argh);
        denom=1+exp(-2*argh);
        G=coeff.*frac(numer,denom);

        bool=(omega==-fc);
        if length(z)==1
            G(bool)=frac(2,rho*abs(fc)*delta.^2).*(h-z);
        else
            G(bool)=frac(2,rho*abs(fc)*delta.^2).*(h-z(bool));
        end
    elseif delta==0
        %         if z==0
        %             coeff=frac(4,rho*abs(fc)*mu);
        %             argz=2*sqrt(2)*rot(s*pi/4).*sqrt(frac(z,mu).*abs(1+omega./fc));
        %             argh=2*sqrt(2)*rot(s*pi/4).*sqrt(frac(h,mu).*abs(1+omega./fc));
        %
        %             [k0z,i0z,k0h,i0h]=bessels_noslip(argz,argh);
        %             length(find(isnan(k0z)))
        %             length(find(isnan(i0z)))
        %             length(find(isnan(k0h)))
        %             length(find(isnan(i0h)))
        %             length(find(isnan(argh)))
        %
        %             %figure,plot(abs(argh))
        %
        %             G=-coeff.*(log(xi0)+frac(1,2)*(z./zo)+frac(k0h,i0h));
        %             G=-coeff.*(log(xi0./xih)+frac(1,2)*(z./zo));
        %             length(find(isnan(xi0./xih)))
        %
        %         else
        %finite-layer Madsen
        coeff=frac(4,rho*abs(fc)*mu);
        argz=2*sqrt(2)*rot(s*pi/4).*sqrt(frac(z,mu).*abs(1+omega./fc));
        argh=2*sqrt(2)*rot(s*pi/4).*sqrt(frac(h,mu).*abs(1+omega./fc));
        [k0z,i0z,k0h,i0h]=bessels_noslip(argz,argh);
        G=coeff.*(k0z-i0z.*frac(k0h,i0h));

        bool=(omega==-fc);
        if length(z)==1
            G(bool)=frac(1,2)*coeff.*log(h./z);
        else
            G(bool)=frac(1,2)*coeff.*log(h./z(bool));
        end
        %end
    else
        %General solution
        G=windtrans_general_noslip(delta,fc,rho,z,mu,omega,h,str);
    end
end

if mu==0
    s=sign(fc).*sign(1+omega./fc);
    Gamma=sqrt(2).*rot(s*pi/4).*sqrt(abs((1+omega./fc)));
    ddelta1=(Gamma.*frac(h,delta).*tanh(Gamma.*frac(h,delta))-1).*G./delta;
    numer=exp(Gamma.*frac(-z,delta))+exp(-Gamma.*frac(2*h-z,delta));
    denom=1+exp(-Gamma.*frac(2*h,delta));
    ddelta2=-frac(2,delta.^2.*abs(fc).*rho).*frac(h-z,delta).*frac(numer,denom);
    ddelta=ddelta1+ddelta2;
    dh1=-Gamma.*frac(1,delta).*tanh(Gamma.*frac(h,delta)).*G;
    dh2=frac(2,delta.^2.*abs(fc).*rho).*frac(numer,denom);
    dh=dh1+dh2;
else
    ddelta=[];
    dh=[];
end

function[G]=windtrans_elipot_noslip(delta,fc,rho,z,mu,omega,h)

zo=delta.^2./mu;
K0=frac(1,2)*delta.^2.*abs(fc);
K1=frac(1,2)*mu*abs(fc);

delta1=sqrt(2.*K0./(omega+fc));
delta2=K1./(omega+fc);
xiz=2*sqrt(1i*(zo+z)./delta2);
xi0=2*sqrt(1i*zo./delta2);
xih=2*sqrt(1i*(zo+h)./delta2);

if h==inf
    if K1==0
       %Ekman solution
        coeff=frac(1,rho.*sqrt(1i*(omega+fc).*K0));
        G=coeff.*exp(-z.*(1+1i)./delta1);
    elseif K0==0
        %Madsen solution
        coeff=(2./rho./K1);
        k0z=besselk(0,2.*sqrt(1i.*z./delta2));
        G=coeff.*k0z;
    else
        %Mixed solution
        coeff=frac(1,rho.*sqrt(1i.*(omega+fc).*K0));
        G=coeff.*besselk(0,xiz)./besselk(1,xi0);
    end
else
    if K1==0
        %finite-layer Ekman
        coeff=frac(1,rho.*sqrt(1i*(omega+fc).*K0));
        numer=sinh((1+1i).*(h-z)./delta1);
        denom=cosh((1+1i).*h./delta1);
        G=coeff.*numer./denom;
    elseif K0==0
        %finite-layer Madsen
        coeff=(2./rho./K1);
        k0z=besselk(0,2.*sqrt(1i.*z./delta2));
        k0h=besselk(0,2*sqrt(1i.*h./delta2));
        i0z=besseli(0,2*sqrt(1i.*z./delta2));
        i0h=besseli(0,2*sqrt(1i.*h./delta2));
        G=coeff.*(k0z-frac(k0h.*i0z,i0h));
    else
        %General solution
        coeff=frac(1,rho.*sqrt(1i.*(omega+fc).*K0));
        [k0z,i0z,k0h,i0h,k10,i10]=bessels_noslip(xiz,xih,xi0);
        numer=i0h.*k0z-k0h.*i0z;
        denom=i10.*k0h+k10.*i0h;
        G=coeff.*frac(numer,denom);
    end
end

function[]=windtrans_test
windtrans_test_gradient;
windtrans_test_limits;

function[]=windtrans_test_gradient

delta=10.^(-1:0.05:3);
h=10.^(log10(15.15):.05:5);
[dg,hg]=meshgrid(delta,h);

[G,G1,G2,G3,G4,dg1,dg2]=vzeros(size(dg));
ddelta=1e-6;dh=1e-6;
omega=-1e-4;
%omega=-1.5e-4;
%omega=-5e-4;
for j=1:length(delta)
    for i=1:length(h)
        %G=windtrans(omega,z,fc,delta,mu,h);
       [G(i,j),dg1(i,j),dg2(i,j)]=windtrans(omega,15,1e-4,dg(i,j),0,hg(i,j));
        G1(i,j)=windtrans(omega,15,1e-4,dg(i,j)-ddelta/2,0,hg(i,j));
        G2(i,j)=windtrans(omega,15,1e-4,dg(i,j)+ddelta/2,0,hg(i,j));
        G3(i,j)=windtrans(omega,15,1e-4,dg(i,j),0,hg(i,j)-dh/2);
        G4(i,j)=windtrans(omega,15,1e-4,dg(i,j),0,hg(i,j)+dh/2);
    end
end

dg1hat=frac(G2-G1,ddelta);
dg2hat=frac(G4-G3,dh);

bool=dg1hat~=0;
eps1=maxmax(log10(frac(abs(dg1hat(bool)-dg1(bool)),sqrt(squared(dg1hat(bool))+squared(dg1(bool))))));
bool=dg2hat~=0;
eps2=maxmax(log10(frac(abs(dg2hat(bool)-dg2(bool)),sqrt(squared(dg2hat(bool))+squared(dg2(bool))))));

%jpcolor(log10(delta),log10(h),real(dg1))
%figure,jpcolor(log10(delta),log10(h),log10(frac(abs(dg1hat-dg1),sqrt(squared(dg1hat)+squared(dg1)))))
%jpcolor(log10(delta),log10(h),log10(frac(abs(dg2hat-dg2),sqrt(squared(dg2hat)+squared(dg2)))))

reporttest(['WINDTRANS analytic and numerical gradients match for Ekman case'],eps1<-4&&eps2<-4)

function[]=windtrans_test_limits

N=1000;
z=[0.1:1:100];
h=[inf 200];
K0=[0 1/10 1/10];
K1=[1 0 1];
fc=1e-4;
delta=sqrt(frac(2*K0,fc));
mu=frac(2*K1,fc);

[bool1,bool2,bool3,bool4]=vzeros(length(K0),2);
[Ge,Go,Gl,Ga]=vzeros(N,length(z),length(K0),2);
slipstr='noslip';
omega=fourier(1,N,'two');

for s=[1 -1]
    for i=1:length(K0)
        for j=1:2
            Ge(:,:,i,j)=windtrans(omega,z,s/2,delta(i),mu(i),h(j),'elipot',slipstr);
            Gl(:,:,i,j)=windtrans(omega,z,s/2,delta(i),mu(i),h(j),slipstr);
            Go(:,:,i,j)=windtrans(omega,z,s/2,delta(i)+1e-14,mu(i)+0.5,h(j),slipstr,'general');
            Ga(:,:,i,j)=windtrans(omega,z,s/2,delta(i)+1e-14,mu(i)+0.5,h(j),slipstr,'two');
            Ge(:,:,i,j)=Ge(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
            Go(:,:,i,j)=Go(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
            Ga(:,:,i,j)=Ga(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
            Gl(:,:,i,j)=Gl(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));

            bool1(i,j)=aresame(Ge(:,:,i,j),Gl(:,:,i,j),1e-8);
            bool2(i,j)=aresame(Ge(:,:,i,j),Go(:,:,i,j),1e-1);
            bool3(i,j)=aresame(Gl(:,:,i,j),Go(:,:,i,j),1e-1);
            bool4(i,j)=aresame(Ga(:,:,i,j),Go(:,:,i,j),1e-20);
        end
    end

    if s==1
        reporttest(['WINDTRANS Elipot and Lilly forms match for f>0 and ' slipstr ' condition'],allall(bool1))
        reporttest(['WINDTRANS general and special forms match for f>0 and ' slipstr ' condition'],allall(bool2).*allall(bool3))
        reporttest(['WINDTRANS general and expansion forms match for f>0 and ' slipstr ' condition'],allall(bool4).*allall(bool3))
    else
        reporttest(['WINDTRANS Elipot and Lilly forms match for f<0 and ' slipstr ' condition'],allall(bool1))
        reporttest(['WINDTRANS general and special forms match for f<0 and ' slipstr ' condition'],allall(bool2).*allall(bool3))
        reporttest(['WINDTRANS general and expansion forms match for f>0 and ' slipstr ' condition'],allall(bool4).*allall(bool3))
    end
end

% older test for freeslip forms; not currently  particularly relevant
% h=20;
% z=[0.1:1:20];
% [bool1,bool2,bool3]=vzeros(length(K0),1);
% [Ge,Go,Gl]=vzeros(N,length(z),length(K0));
% slipstr='freeslip';
% for s=[1 -1]
%     for i=1:length(K0)
%         for j=1:length(h)
%             i,j,s
%             Ge(:,:,i,j)=windtrans(omega,z,s/2,delta(i),mu(i),h(j),'elipot',slipstr);
%             Gl(:,:,i,j)=windtrans(omega,z,s/2,delta(i),mu(i),h(j),'lilly',slipstr);
%             Go(:,:,i,j)=windtrans(omega,z,s/2,delta(i)+1e-10,mu(i)+0.5,h(j),slipstr,'general');
%             Ge(:,:,i,j)=Ge(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
%             Go(:,:,i,j)=Go(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
%             Gl(:,:,i,j)=Gl(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
%             bool1(i,j)=aresame(Ge(:,:,i,j),Gl(:,:,i,j),1e-8);
%             bool2(i,j)=aresame(Ge(:,:,i,j),Go(:,:,i,j),1e-1);
%             bool3(i,j)=aresame(Gl(:,:,i,j),Go(:,:,i,j),1e-1);
%         end
%     end
%
%     if s==1
%         reporttest(['WINDTRANS Elipot and Lilly forms match for f>0 and ' slipstr ' condition'],allall(bool1))
%         reporttest(['WINDTRANS general and special forms match for f>0 and ' slipstr ' condition'],allall(bool2).*allall(bool3))
%     else
%         reporttest(['WINDTRANS Elipot and Lilly forms match for f<0 and ' slipstr ' condition'],allall(bool1))
%         reporttest(['WINDTRANS general and special forms match for f<0 and ' slipstr ' condition'],allall(bool2).*allall(bool3))
%     end
% end


 function [deriv, curv] = trl_dercurv(data, li, curvtype, iblock, nblock)
%function [deriv, curv] = trl_dercurv(data, li, curvtype, iblock, nblock)
%
% evaluate derivatives and curvatures for monoenergetic Poisson transmission
% negative log-likelihoods: f(x) = \sum_i h_i(l), h_i(l) = mi(l) - yi log mi(l)
% mi(l) = bi exp(-l) + ri
%
% in
%	data	{yi, bi, ri}
%				yi is [nb,na].  bi, ri are same or scalar
%				passed to trl_curvature()
%	li	[nb,#view_in_block]
%	curvtype ''		curvature type
%	iblock, nblock		for OS type methods
%
% out
%	deriv	[nb,#]		\dot hi(l)
%	curv	[nb,#]		surrogate curvature for hi at li
%
% Copyright 2004-2-1, Jeff Fessler, The University of Michigan

if nargin < 2, ir_usage, end
if nargin < 3, curvtype = 'pc'; end

yi = data{1};
bi = data{2};
ri = data{3};

if nargin == 5
	ia = iblock:nblock:size(yi,2);
	yi = yi(:,ia);
	if length(bi) > 1
		bi = bi(:,ia);
	end
	if length(ri) > 1
		ri = ri(:,ia);
	end
end

% transmission Poisson likelihood function
ei = exp(-li);
mi = bi .* ei + ri;
deriv = (1 - yi ./ mi) .* (-bi .* ei);

curv = trl_curvature(yi, bi, ri, li, curvtype);


function [out] = area_1(p1,p2,S,Smin,Smax,varargin)
%area_1

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Auxiliary function that calculates a variable contributing area.
% Constraints:  A <= 1
% @(Inputs):    p1   - linear scaling parameter [-]
%               p2   - exponential scaling parameter [-]
%               S    - current storage [mm]
%               Smin - minimum contributing storage [mm]
%               Smax - maximum contributing storage [mm]
%               varargin(1) - smoothing variable r (default 0.01)
%               varargin(2) - smoothing variable e (default 5.00)

if size(varargin,2) == 0
    out = min(1,p1.*(max(0,S-Smin)./(Smax-Smin)).^p2).*...
            (1-smoothThreshold_storage_logistic(S,Smin));                       % default smoothing
elseif size(varargin,2) == 1
    out = min(1,p1.*(max(0,S-Smin)./(Smax-Smin)).^p2).*...
            (1-smoothThreshold_storage_logistic(S,Smin,varargin(1)));           % user-specified smoothing
elseif size(varargin,2) == 2
     out = min(1,p1.*(max(0,S-Smin)./(Smax-Smin)).^p2).*...
            (1-smoothThreshold_storage_logistic(S,Smin,varargin(1),varargin(2))); % user-specified smoothing
end

end


function [ Tt ] = updateTransform2(T,w,v,dt)
% This function takes the input of the
normw=norm(w);
if normw>0
    R1=T(1:3,1:3);
    wCrossR1=zeros(3,3);
    for i=1:3
        wCrossR1(:,i)=cross(w,R1(:,i));
    end
    Rt=R1+wCrossR1*dt;
else
    Rt=T(1:3,1:3);
end

dx=v*dt;
xt=T(1:3,4)+dx;

Tt=[Rt,xt];
Tt=[Tt;
    0 0 0 1];
Tt=normalizeTransformMatrix(Tt);
end

function R=quat2rot(q)
R=[(1-2*q(3)*q(3)-2*q(4)*q(4)), 2*(q(1)*q(3)-q(1)*q(4)), 2*(q(2)*q(4)+q(1)*q(3));
2*(q(2)*q(3)+q(1)*q(4)), (1-2*q(2)*q(2)-2*q(4)*q(4)), 2*(q(3)*q(4)-q(1)*q(2));
2*(q(2)*q(4)-q(1)*q(3)), 2*(q(3)*q(4)+q(1)*q(2)), (1-2*q(2)*q(2)-2*q(3)*q(3))];
end

function T=normalizeTransformMatrix(t)
T=zeros(4,4);
T(:,4)=t(:,4);
for i=1:3
    T(1:3,i)=t(1:3,i)/norm(t(1:3,i));
end
end


function [A, B] = reflexKronApprox( K, m, n, Rtol )
%
%       [A, B] = reflexKronApprox( K, m, n, Rtol );
%
% computes a kronecker sum approximation to the blurring matrix K
% that arises from the input PSF under reflexive boundary conditions.
% This approximation is done a-la Nagy-Ng-Perrone (see paper for details).
%
%  Input:
%         K - psfMatrix object
%         m - size of matrix A (assumed square)
%         n - size of matrix B (assumed square)
%         Rtol - relative tolerance ( <=1 ); determines number of kron products
%                in the approximation.  If si/s1 > Rtol,
%                where si is the ith largest
%                singular value of the weighted PSF,
%                then Ai(x)Bi will be included in the approximation.
%                IF ONLY ONE TERM IN THE SUM IS DESIRED, USE Rtol = 1.
%
%  Output:
%      Cells A and B such that K \approx \sum_i[ A{i} \otimes B{i} ].
%

%  L. Perrone, 4/28/02

%  Modifications:
%  5/25/02, J. Nagy
%           Cosmetic changes to incorporate into RestoreTools
%
%  9/??/02  L. Perrone
%           This code now conforms to the new kronMatrix class,
%           where fields K.a and K.b are cell arrays containing
%           (possibly) more than one matrix, depending on Rtol.
%  11/22/02 L. Perrone
%           The kronMatrix class should work for image processing
%           problems, where it is common to use lexicographical (row)
%           ordering.  However, the kronMatrix class was designed
%           using vec(column) ordering (unlike all other classes
%           in the RestoreTools package).  Until now, the inconsistency
%           remained hidden because the PSFs in use
%           had been symmetric or close to symmetric.
%           This discrepancy has now been fixed.

P1 = K.psf;
P2 = P1.image;
PSF = P2{1};
c1 = P1.center;
center = c1{1};

[mp, np] = size(PSF);

if ( mp ~= np )
  error('For now, we expect PSF to be square')
end

%
% Compute weighted PSF.
%
c = zeros(mp,1);
c(1) = mp;
c(2:2:end) = 1;
R = chol( toeplitz(c) );

Phat = R*PSF*R';

%
% Compute SVD of weighted PSF, which is then used to construct
% the separable approximation.
%
[U,S,V] = svd( Phat );

%
% check to make sure first column looks like
% a Gaussian, and is not inverted.
%
minU = abs(min(min(U(:,1))));
maxU = max(max(abs(U(:,1))));
if minU == maxU
  U = -U;
  V = -V;
end

%
% Construct approximation.
%
A = cell(1);
B = cell(1);
i=1;
PP = zeros(size(PSF));
while S(i,i)/S(1,1) >= Rtol
  a = R \ ( U(:,i) * sqrt(S(i,i)) );
  b = R \ ( V(:,i) * sqrt(S(i,i)) );
  PP = PP + a*b';
  % Comment out the next two statements, replace with the two that
  % follow, in order to fix the 11/22/02 problem.
  %A{i} = build_toep(a, center(1), m) + buildHank(a, center(1), m);
  %B{i} = build_toep(b, center(2), n) + buildHank(b, center(2), n);
  B{i} = build_toep(a, center(1), m) + buildHank(a, center(1), m);
  A{i} = build_toep(b, center(2), n) + buildHank(b, center(2), n);
  i=i+1;
end

%
%  The following scales the approximation so that the
%  corresponding PSF approximation has the property
%  that its sum of values is the same as the sum of the
%  original PSF values.
%
cs = sqrt(sum(PSF(:))/sum(PP(:)));
for i = 1:length(A)
  A{i} = cs*A{i};
  B{i} = cs*B{i};
  AA = A{i};
  BB = B{i};
end