% @author: Maziar Raissi

function params = Fractional_Levy()

clc; close all;

plt = 1;
plt_pred = 0;
save_plt = 1;

addpath ..
addpath ../Utilities
addpath ../Kernels/Fractional_Levy
addpath ../Utilities/export_fig

function CleanupFun()
    rmpath ..
    rmpath ../Utilities
    rmpath ../Kernels/Fractional_Levy
    rmpath ../Utilities/export_fig
end

finishup = onCleanup(@() CleanupFun());

rng('default')

set(0,'defaulttextinterpreter','latex')

%% Load Data
[t_star, x_star, u_star, pos] = gen_data(10^6, 0.01, 5, 100);
% u_star --> 100x5
% t_star --> 5x1
% x_star --> 100x1
N_star = size(x_star,1);
nsteps = size(t_star,1)-1;

%% Setup
N0 = 100;
N1 = 100;
%% Clean Data
i = 4;
dt = t_star(i+1) - t_star(i);

idx0 = randsample(N_star, N0);
x0 = x_star(idx0,:);
u0 = u_star(idx0,i);

idx1 = randsample(N_star,N1);
x1 = x_star(idx1,:);
u1 = u_star(idx1,i+1);

hyp = [log([1.0 0.1]) 1.1 0.0];
model = HPM(x1, u1, x0, u0, dt, hyp);
model = model.train(1500);

hyp = model.hyp;
params = hyp(3);

[pred_n_star, var_n_star] = model.predict(x_star);
var_n_star = abs(diag(var_n_star));

error = norm(pred_n_star - u_star(:,i+1))/norm(u_star(:,i+1));

fprintf(1,'=========================\n');
fprintf(1,'Step: %d, Time = %.2f\n\nNLML = %.2f, Error = %.2e\n\n', i, ...
    t_star(i+1), model.NLML, error);

str = sprintf('%.2f  ', params);
fprintf('Parameters: %s\n\n', str)
fprintf(1,'=========================\n\n');

if plt_pred == 1
    figure();
    plot_prediction_1D(x_star, u_star(:,i+1), pred_n_star, var_n_star, ...
        '$x$', '$u(t,x)$', 'Prediction');

    drawnow;
end

%% Plot Results

if plt == 1
    fig = figure();
    set(fig,'units','normalized','outerposition',[0 0 1 .5])
    subplot(3,2,1:2)
    plot(pos, 'b', 'LineWidth', 2);
    xlabel('Time')
    ylabel('Position')
    axis tight
    set(gca,'FontSize',14);
    set(gcf, 'Color', 'w');

    subplot(3,2,3);
    tit = sprintf('$t = $ %.2f\n%d training data\n', t_star(i), N0);
    bar(x_star, u_star(:,i),'m');
    xlabel('$x$')
    ylabel('$u(t,x)$')
    axis tight
    set(gca,'FontSize',14);
    set(gcf, 'Color', 'w');
    title(tit);

    subplot(3,2,4);
    tit = sprintf('$t = $ %.2f\n%d training data\n', t_star(i+1), N1);
    bar(x_star, u_star(:,i+1),'m');
    xlabel('$x$')
    ylabel('$u(t,x)$')
    axis tight
    set(gca,'FontSize',14);
    set(gcf, 'Color', 'w');
    title(tit);

    subplot(3,2,5:6);

    s = '$\begin{tabular}{|c|c|}';
    s = strcat(s, ' \hline');
    s = strcat(s, ' Correct PDE & $u_t + (-\nabla^{\sqrt{2}}_x) u = 0$ \\');
    s = strcat(s, ' \hline');
    s = strcat(s, ' Identified PDE & $u_t + (-\nabla^{', sprintf('%.3f',params(1)), '}_x) u = 0$ \\');
    s = strcat(s, ' \hline');
    s = strcat(s, ' \end{tabular}$');
    text(0.25,0.8,s,'interpreter','latex','FontSize',18)
    axis off

    if save_plt == 1
        export_fig ../Figures/Fractional_Levy.png -r300
    end

    drawnow();
end

end

function [t_star, x_star, u_star, pos] = gen_data(N,dt,m,n)

    alpha = sqrt(2);
    r = stblrnd(alpha, 0, 1, 0, N, 1);
    pos = cumsum(dt^(1/alpha)*r);

    M = 0.35;

    P = zeros(length(pos)-m,m);
    for i = 1:length(pos)-m
        y = pos(i+1:i+m) - pos(i);
        P(i,:) = y;
    end

    bins = linspace(-M,M,n+1);
    x_star = linspace(M*(1/n-1), M*(1-1/n), n)';
    t_star = dt*(1:1:m)';
    u_star = zeros(n,m);

    f = figure();
    clf
    hold
    for i = 1:m
        h = histogram(P(:,i), bins, 'Normalization', 'pdf');
        u_star(:,i) = h.Values;
    end

    close(f)

end


% =========================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM%
% 2D Multilevel LDDMM Example using stationary velocity field and
% diffusion regularizer.The example is described in detail in
% Section 4.2 of the paper:
%
% @article{MangRuthotto2017,
%   Title = {A {L}agrangian {G}auss--{N}ewton--{K}rylov solver for mass- and intensity-preserving diffeomorphic image registration},
%   Year = {2017},
%   Journal = {SIAM Journal on Scientific Computing},
%   Author = {A. Mang, L. Ruthotto},
% }
% =========================================================================

close all; clear all; clc;
setup2Ddisc2CData

%% run affine pre-registration
imgModel('reset','imgModel','splineInterMex','regularizer','moments','theta',.1);

alpha = [4e2 0];
parametric = 0;
pad  = .5;
N    = 3;
mV     = @(m) ceil(1*m);
minLevel = 5;
maxLevel = 7;

%% run multilevel LDDMM registration

% 1) setup grid for velocities (padded)
omegaV = omega; omegaV(1:2:end) = omegaV(1:2:end)-pad;  omegaV(2:2:end) = omega(2:2:end)+pad;

% 2) setup regularizer
regularizer('reset','regularizer','mbDiffusionCC','nt',0,'alpha',alpha,'HessianShift',1e-2);

NPIRpara         = optPara('NPIR-GN');
NPIRpara.maxIter = 40;
NPIRpara.scheme  = @GaussNewtonLDDMM;
[vc,~,wc,his] = MLLDDMM(ML,'minLevel',minLevel,'maxLevel',maxLevel,...
    'omegaV',omegaV,'mV',mV,'N',N,'parametric',parametric,'NPIRpara',NPIRpara,'plots',1);

%% show results
yc = getTrafoFromVelocityRK4(vc,getNodalGrid(omega,m),'omega',omegaV,'m',m,'tspan',[1,0],'N',N);
Topt = linearInterMex(dataT,omega,center(yc,m));
Jac = geometry(yc,m,'Jac','omega',omega);
D0  = distance(dataT(:),dataR(:),omega,m);
DOpt = distance(Topt(:),dataR(:),omega,m);

fig = figure(); clf;
fig.Name = sprintf('LDDMM Results: %s',mfilename);

subplot(2,3,1);
viewImage(dataR,omega,m);
title('reference');

subplot(2,3,4);
viewImage(dataT,omega,m);
hold on;
plotGrid(yc,omega,m,'spacing',4)
title('template');

subplot(2,3,2);
viewImage(Topt,omega,m);
title('T(yc)')

subplot(2,3,3);
viewImage(dataT(:)-dataR(:),omega,m);
title('init. residual, SSD=100%');

subplot(2,3,5);
viewImage2Dsc(Jac,omega,m);
title(sprintf('Jac, min=%1.2f max=%1.2f',min(Jac(:)),max(Jac(:))));

subplot(2,3,6);
viewImage(Topt(:)-dataR(:),omega,m);
title(sprintf('opt residual, SSD=%1.2f%%',100*DOpt/D0));


function [k, rbfPart, n2] = sqexpKernCompute(kern, x, x2)

% SQEXPKERNCOMPUTE Compute the SQEXP kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the pre-built compound squared exponential
% kernel given inputs associated with rows and columns.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : the input matrix associated with the rows of the kernel.
% ARG x2 : the input matrix associated with the columns of the kernel.
% RETURN k : the kernel matrix computed at the given points.
%
% FORMAT
% DESC computes the kernel matrix for the pre-built compound squared exponential
% kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : the kernel matrix computed at the given points.
%
% SEEALSO : sqexpKernParamInit, kernCompute, kernCreate, sqexpKernDiagCompute
%
% COPYRIGHT : Neil D. Lawrence, 2004

% KERN

if nargin < 3
  n2 = dist2(x, x);
  wi2 = (.5 .* kern.inverseWidth);
  rbfPart = kern.rbfVariance*exp(-n2*wi2);
  k = rbfPart + kern.whiteVariance*eye(size(x, 1));
else
  n2 = dist2(x, x2);
  wi2 = (.5 .* kern.inverseWidth);
  rbfPart = kern.rbfVariance*exp(-n2*wi2);
  k = rbfPart;
end
k = k + kern.biasVariance;


function [C, S] = QLiftDec2MinMin(X, N)
%-----------------------------------------------------------------------------
% QLiftDec2MinMin
% Multilevel 2-D decomposition by the lifting scheme and using quincunx grids
%
% The MinMin scheme has been proposed by Heijmans and Goutsias, see e.g.
%    H.J.A.M. Heijmans, J. Goutsias,
%    Multiresolution signal decomposition schemes.
%    Part 2: morphological wavelets.
%    CWI Report PNA-R9905, Amsterdam, 1999.
%    http://repository.cwi.nl:8888/cwi_repository/docs/IV/04/04625D.pdf
%
% Calls for: QLmaxlev,
%            storeQ1001, storeR,
%            getcolor01, getcolor10, getcolor00, getcolor11,
%            putcolor01, putcolor10, putcolor00, putcolor11.
% See also: QLiftRec2MaxMin
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: February 3, 2003.
% (c) 1999-2003 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
%Firstly, check input data
%
if  isempty(X)
  error(' QLiftDec2MinMin - empty matrix ');
else
  if mod(N, 2) == 1
    error(' QLiftDec2MinMin - only an even number of levels is accepted ');
  end
  if QLmaxlev(size(X), 'minmin') < N
    error(' QLiftDec2MinMin - too many levels requested ');
  end
  if N < 2
    disp([' QLiftDec2MinMin - WARNING too few levels requested ' ...
          '-> empty decomposition ']);
  end
end
%
%Secondly, start decomposition
%
O = X; % For the sake of efficient use of memory this could be improved upon.
% We descend to coarser grids, integer lev indicates number of scale.
C = []; S = [];

for lev=1:2:N
%
   [nO, mO] = size(O);
   if ( nO < 3 ) || ( mO < 3)
     error(' QLiftDec2MinMin - too many levels ');
   end
   minO = min(min(O));
   maxO = max(max(O));
%  cmin = minO-(maxO-minO);
   cmax = maxO+(maxO-minO);
%
%  The Lifting Scheme proceeds from a rectangular grid
%  towards a quincunx grid.
%
%  Stage: predict
   A00 =getcolor00(O);
   A11 =getcolor11(O);
%  Quincunx grid Q0011 is the union of the values at .00 and .11: "even slots"
%  Quincunx grid Q1001 is the union of the values at .10 and .01: "odd slots"
   Q1001D01 = getcolor01(O) - synA01min(A11, A00, cmax);                % Y1
   Q1001D10 = getcolor10(O) - synA10min(A11, A00, cmax);                % Y1
%  At this point the union (quincunx) of Q1001D01 & Q1001D10
%  contains the DETAILS of O.
%
%  For the inverse transform Q1001D01 and Q1001D10 have to be stored:
   [C, S] = storeQ1001( Q1001D10, Q1001D01, lev, 'd', C, S);
%
%  Stage: update
   Q0011A00 = A00 + ...
        min(zeros(size(A00)), synA00min(Q1001D10, Q1001D01, cmax));     % X1
   clear A00;
   Q0011A11 = A11 + ...
        min(zeros(size(A11)), synA11min(Q1001D10, Q1001D01, cmax));     % X1
   clear A11 Q1001D10 Q1001D01;
%  At this point the union (quincunx) of Q0011A00 & Q0011A11
%  contains the updated APPROXIMATION of O, the DETAILS of O
%  were in the union (quincunx) of Q1001D01 & Q1001D10 (see above).
%
%  The Lifting Scheme proceeds by a subsequent step from quincunx
%  to rectangular grid.
%
%  Q0011 is split into the 11 colour with the "odd slots" and
%  the 00 colour with the "even slots".
%
%  Stage: predict
   DETAIL11 = Q0011A11 - synA11Qmin(Q0011A00, size(Q0011A11), cmax);        % Y2
   clear Q0011A11;
%  Stage: update
   APPROX00 = Q0011A00 + ...
     min(zeros(size(Q0011A00)), synA00Qmin(DETAIL11, size(Q0011A00), cmax));% X2
%
%  DETAIL11 presents the detail gridfunction w.r.t. Q0011
%  APPROX00 now represents the updated version of the approximation of Q0011
%
%  For the inverse transform DETAIL11 has to be stored:
   [C, S] = storeR( DETAIL11, lev+1, 'd', C, S);
   clear Q0011A00 DETAIL11;
%
%  At this point gridfunction DETAIL11 containing the DETAILS has been stored,
%  gridfunction APPROX00 contains the updated APPROXIMATION, on the (down-
%  sampled) rectangular grid and has to be stored as well if at the highest
%  scale.
%  Note that APPROX00 is downsampled onto a rectangular grid with dimensions of
%  half size of the original O.
   if lev+1 >= N
     [C, S] = storeR(APPROX00, lev+1, 'a', C, S);
%    It is obligatory that at least at one scale the Approximation has to be
%    stored or else the scheme cannot be inverted.
     clear APPROX00;
%    In the Lifting Scheme all scales have now been processed!
   else
%    We proceed to the next scale.
     O = APPROX00; clear APPROX00;
   end
end
%-----------------------------------------------------------------------------


function [psd, ft] = gsp_jtv_estimate_psd(G, X, param)
%GSP_JTV_ESTIMATE_PSD Estimate the PSD of a time vertex process
%   Usage:  psd = gsp_jtv_estimate_psd(G, X)
%           psd = gsp_jtv_estimate_psd(G, X, param)
%
%   Input parameters:
%         G          : Graph
%         X          : Signal(s) (matrix of NxTxR)
%         param      : Optional parameters
%   Output parameters:
%         psd        : PSD matrix
%         ft         : Filter type
%
%   This function estimates the PSD for vertex time processes. It is a
%   generalization of the Bartlett method.
%
%   Additional parameters
%   ---------------------
%
%   * *param.estimator*  : Method used for the estimation. By default 'TA'
%     - 'TA': Average over the time domain.
%     - 'FC': Fourier convolution a trick used to reduce the computation.
%     - 'VA': Average over the vertex domain
%     - 'TVA': Average over the time and the vertex domain
%     - 'FA' : Average on multiple realization in the joint spectral domain
%   * *param.L*       : Length of the window for the 'GSTFT' method
%   * *param.kernel*  : Kernel for the convolution for 'FC' method.
%
%
%   References: perraudin2016stationary

% Author : Nathanael Perraudin
% Date: 6 January 2016
% Testing: test_gsp_estimate_vertex_time_psd

if nargin<3
    param = struct;
end
if ~isfield(param,'estimator')
    param.estimator = 'FA';
end

% number of realizations
[N, T, R] = size(X);

switch param.estimator

    % This method compute the Fourier transform of the signals and average
    % the modulus
    case 'FA'
        if ~isfield(param, 'L')
            param.L = compute_default_L(X,T,R);
        end
        Xhat = gsp_jft(G,reshape(X,N,param.L,R*T/param.L));
        psd = mean(abs(Xhat).^2,3);
        ft = 'js';

    % This method splits time in windows, and takes an average of the psd
    % over each time window. The size of each window (L) should be
    % equal to the maximum correlation distance in the process
    case 'TA'

        if ~gsp_check_fourier(G)
            error('The Fourier basis is needed the Fourier basis')
            %G = gsp_compute_fourier_basis(G);
        end

        if ~isfield(param, 'L')
            param.L = compute_default_L(X,T,R);
        end
        if ~isfield(param, 'a'),       param.a = round(param.L/2); end
        if ~isfield(param, 'M'),       param.M = G.jtv.T; end
        if ~isfield(param,'win_type'), param.win_type = 'itersine'; end

        if round(T/param.L) - (T/param.L) ~= 0
            warning('Optimally the length of the signal should be divisible by param.L.')
            while round(T/param.L) - (T/param.L) ~= 0
                param.L = param.L + 1;
            end
        end

        % Compute a Gabor window
        w = gabwin(param.win_type, param.a, param.L);

        % estimate the psd as the average over each realization
        psd = zeros(G.N, G.jtv.T);
        for r = 1:R

            % do a windowed JFT
            coeff = gsp_jtwgft(G, w, X(:,:,r), param);

            % compute the psd for each window
            psd_win = abs(coeff(:, 1:end-1, :)).^2;

            % average all windows to obtain an estimate of the psd
            psd_est = squeeze(mean(psd_win, 2));

            % average over realizations
            psd = psd + transpose(psd_est);
        end

        psd = psd / R;

        % normalize energy
        psd = psd * (norm(X(:), 'fro')^2/R / sum(psd(:)));

    % Go to the joint frequency domain and smooth out the PSD with a kernel.
        ft = 'js';

    case 'FC'
        if ~isfield(param, 'kernel')
            param.kernel = exp(-(-20:20).^2/3);
        end
        h = param.kernel;

        if ~gsp_check_fourier(G)
            G = gsp_compute_fourier_basis(G);
        end

        Xhat2 = abs(gsp_jft(G,X)).^2;

        psd = conv2(h', h', mean(Xhat2,3), 'same') / norm(h, 1)^2;
%         psd = conv2(mean(Xhat2,3), param.kernel, 'same') / norm(param.kernel, 1);

        % normalize energy
        psd = psd * (norm(X(:), 'fro')^2/R / sum(psd(:)));
        ft = 'js';

    % For scalability,
    case 'VA'

        if ~isfield(G,'boundary'); param.boundary = 'periodic'; end

        switch param.boundary

            case 'periodic'
                Xhat = fft(X,[],2)/sqrt(T);

            case 'reflecting'
                error('Sorry. Not implemented yet...')

            otherwise
                error('Unknown boundary condition');
        end

        if ~isfield(G,'lmax')
            G = gsp_estimate_lmax(G);

            warning(['GSP_PSD_ESTIMATION: The variable lmax is not ',...
                'available. The function will compute it for you. ',...
                'However, if you apply many time this function, you ',...
                'should precompute it using the function: ',...
                'gsp_estimate_lmax']);
        end

        if ~isfield(param,'Nfilt'),   param.Nfilt = 50; end
        if ~isfield(param,'Nrandom'), param.Nrandom = max(10, R); end
        if ~isfield(param,'g0')
            sigma = sqrt(2*G.lmax/param.Nfilt^2 * (param.Nfilt + 1));
            param.g0 = @(x) exp(-x.^2/sigma^2);
        end

        % Design the frame
        [ g , mu ] = gsp_design_translates(G, param.g0, param.Nfilt);
        %[ g , mu ] = gsp_design_itersine(G,Nfilt );

        % Estimate the energy of the window
        if gsp_check_fourier(G)
            mu_y2 = sum(abs(gsp_filter_evaluate(g,G.e)).^2,1)';
        else
            w = randn(N, param.Nrandom);
            x_filt2    = gsp_vec2mat( gsp_filter(G,g,w,param), param.Nfilt);
            n2_x_filt2 = sum(abs(x_filt2).^2,1);
            mu_y2      = reshape(mean(n2_x_filt2,3),[],1);
        end

        psd = cell(1,T);

        for ii = 1:T
            % Perform the filtering
            x_filt = gsp_vec2mat( ...
                gsp_filter(G, g, squeeze(Xhat(:,ii,:)), param), ...
                param.Nfilt);
            % estimate the points
            n2_x_filt = sum(abs(x_filt).^2,1);
            mu_y = reshape(mean(n2_x_filt,3),[],1);

            % Interpolate to obtain a nice filter.
            psd{ii} = @(s) max(spline(mu,mu_y./mu_y2,s), 0);
        end
        ft = 'js-array';

   case 'TVA'

        if ~isfield(param, 'L')

            threshold = 0.05;

            % compute the average correlation distance
            C_T = zeros(T); Toep = toeplitz(0:(T-1)); Tmax = round(0.5*T);

            for r = 1:R, C_T = C_T + abs(X(:,:,r)'*X(:,:,r)) / R; end
            cor = zeros(Tmax,1);
            for t = 1:Tmax, cor(t) = mean( vec(C_T(Toep == t-1)) ); end
            cor = normalize_data(cor, 0, 1);

            % select the window length conservatively
            param.L = min(4*find(cor>=threshold, 1, 'last' ), T);

            % visualize the correlation
            % figure; plot(0:(Tmax-1), cor, '-o', [1 1]*param.L, [0 1],
            % 'r-'); xlabel('correlation distance');

            % param.L = round(T/4);
        end

       % Make sure that the length of the signal should be divisible by param.L
       if round(T/param.L) - (T/param.L) ~= 0
           % warning('Ideally the length of the signal should be divisible by param.L.')
           while (round(T/param.L) - (T/param.L) ~= 0) || (round(param.L/2) - (param.L/2) ~= 0)
              param.L = param.L + 1;
           end
       end

       if ~isfield(param, 'a'),       param.a = round(param.L/2);  end
       if ~isfield(param, 'M'),       param.M = G.jtv.T;           end
       if ~isfield(param,'win_type'), param.win_type = 'itersine'; end
       if ~isfield(G,'boundary');     param.boundary = 'periodic'; end

       % Compute a Gabor window
       w = gabwin(param.win_type, param.a, param.L);

        switch param.boundary

            case 'periodic'
                S = zeros(N, param.M, ceil(T/param.a), R);
                for r = 1:R
                    % do a discrete gabore transform
                    X_gabore = dgt(transpose(X(:,:,r)), w, param.a, param.M);
                    S(:,:,:,r) = permute( X_gabore, [3,1,2]);
                end
            case 'reflecting'
                error('Sorry. Not implemented yet...')
            otherwise
                error('Unknown boundary condition');
        end

        if ~isfield(G,'lmax')
            G = gsp_estimate_lmax(G);

            warning(['GSP_PSD_ESTIMATION: The variable lmax is not ',...
                'available. The function will compute it for you. ',...
                'However, if you apply many time this function, you ',...
                'should precompute it using the function: ',...
                'gsp_estimate_lmax']);
        end

        if ~isfield(param,'Nfilt'),   param.Nfilt = 50;          end
        if ~isfield(param,'Nrandom'), param.Nrandom = max(10,R); end
        if ~isfield(param,'g0')
            sigma = sqrt(2*G.lmax/param.Nfilt^2 * (param.Nfilt + 1));
            param.g0 = @(x) exp(-x.^2/sigma^2);
        end

        % Design the frame
        [ g , mu ] = gsp_design_translates(G, param.g0, param.Nfilt);
        %[ g , mu ] = gsp_design_itersine(G,Nfilt );

        % Estimate the energy of the window
        if gsp_check_fourier(G)
            mu_y2 = sum(abs(gsp_filter_evaluate(g, G.e)).^2, 1)';
        else
            w = randn(N, param.Nrandom);
            x_filt2 = gsp_vec2mat( gsp_filter_analysis(G,g,w,param), param.Nfilt);
            n2_x_filt2 = sum(abs(x_filt2).^2, 1);
            mu_y2 = reshape(mean(n2_x_filt2,3), [], 1);
        end

        psd = cell(1, param.M);

%         if gsp_check_fourier(G) && param.use_fast
%             warning('This may use a lot of ram!')
%             [N, T, Nt, Ns ] = size(S);
%             Nf = numel(g);
%             ShatG = gsp_gft(G,S);
%             filt_eval = gsp_filter_evaluate(g, G.e);
%             filt_eval = repmat(reshape(filt_eval,[N,1,1,1,Nf]), 1,T,Nt,Ns ,1);
%             % Perform the filtering
%             S_filt = gsp_igft(G, repmat(ShatG,[1,1,1,1,Nf]) .* filt_eval);
%             n2_S_filt = squeeze(mean(mean(sum(abs(S_filt).^2,1),3),4));
%             for ii = 1:param.M
%
%                 % Interpolate to obtain a nice filter.
%                 psd{ii} = @(s) max(spline(mu,reshape(n2_S_filt(ii,:),[],1)./mu_y2,s), 0);
%             end
%         else
        for ii = 1:param.M

            % Perform the filtering
            x_filt = gsp_vec2mat( ...
                gsp_filter(G, g, reshape(S(:,ii,:,:),N,[]), param), ...
                param.Nfilt);

            % estimate the points
            n2_x_filt = sum(abs(x_filt).^2,1);
            mu_y = reshape(mean(n2_x_filt,3),[],1);

            % Interpolate to obtain a nice filter.
            psd{ii} = @(s) max(spline(mu,mu_y./mu_y2,s), 0);
        end
%         end

        ft = 'js-array';

    otherwise
        error('Unknown method')
end

end

function [x] = normalize_data(x, xmin, xmax)
%NORMALIZE_VECTOR Normalize x between xmin and xmax
%   x might be a scalar, vector, or matrix.

%normalize to [0,1]
x = (x - min(min(x))) ./ (max(max(x)) - min(min(x)));

if exist('xmax', 'var') && exist('xmin', 'var')
   x = x .* (xmax - xmin) + xmin;
end

end

function L =  compute_default_L(X,T,R)
% compute the correlation distance
C_T = zeros(T);
for r = 1:R
    C_T = C_T + abs(X(:,:,r)'*X(:,:,r)) / R;
end
Toep = toeplitz(0:(T-1));
Tmax = round(0.5*T);
cor = zeros(Tmax,1);
for t = 1:Tmax
    cor(t) = mean( vec(C_T(Toep == t-1)) );
end
cor = normalize_data(cor, 0, 1);
threshold = 0.05;
L = min(2*find(cor>=threshold, 1, 'last'), T);

%L = round(T/4);
%figure; plot(0:(Tmax-1), cor, '-o', [1 1]*param.L, [0 1], 'r-');

end


function [X,Y,vals,labI]=mp_azim(optn,varargin)
% MP_AZIM Azimuthal projections
%           This function should not be used directly; instead it is
%           is accessed by various high-level functions named M_*.

% Rich Pawlowicz (rich@ocgy.ubc.ca) 2/Apr/1997
%
%         13/5/97 - Added satellite perspective
%          1/6/97 - Another stab at removing some /0 errors.
%         10/8/00 - Rotation for projections?
%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.
%
% Mathematical formulas for the projections and their inverses are taken from
%
%      Snyder, John P., Map Projections used by the US Geological Survey,
%      Geol. Surv. Bull. 1532, 2nd Edition, USGPO, Washington D.C., 1983.
%
% These are azimuthal projections, best suited for circular areas. The
% stereographic is commonly used for polar regions.
%   Stereographic  - conformal
%   Orthographic   - neither conformal nor equal-area, but looks like globe
%                    with viewpoint at infinity.
%   Azimuthal Equal-area  - equal area, but not conformal (by Lambert)
%   Azimuthal Equidistant - distance and direction from center are true
%   Gnomonic       - all great circles are straight lines.
%   Satellite      - a perspective view from a finite distance

global MAP_PROJECTION MAP_VAR_LIST

name={'Stereographic','Orthographic','Azimuthal Equal-area','Azimuthal Equidistant','Gnomonic','Satellite'};

pi180=pi/180;

switch optn

  case 'name'
     X=name;

  case {'usage','set'}
     X=char({['     ''' varargin{1} ''''],...
              '     <,''lon<gitude>'',center_long>',...
              '     <,''lat<itude>'', center_lat>',...
              '     <,''rad<ius>'', ( degrees | [longitude latitude] ) | ''alt<itude>'', alt_frac >',...
              '     <,''rec<tbox>'', ( ''on'' | ''off'' | ''circle'' )>',...
	      '     <,''rot<angle>'', degrees CCW>'});

  case 'get'

     X=char([' Projection: ' MAP_PROJECTION.name '  (function: ' MAP_PROJECTION.routine ')'],...
            [' center longitude: ' num2str(MAP_VAR_LIST.ulong) ],...
            [' center latitude: ' num2str(MAP_VAR_LIST.ulat) ],...
            [' radius/altitude : ' num2str(MAP_VAR_LIST.uradius) ],...
            [' Rectangular border: ' MAP_VAR_LIST.rectbox ],...
	    [' Rotation angle: ' num2str(MAP_VAR_LIST.rotang) ]);

  case 'initialize'

    MAP_VAR_LIST=[];
    MAP_PROJECTION.name=varargin{1};
    MAP_VAR_LIST.ulong=0;
    MAP_VAR_LIST.ulat=60;
    MAP_VAR_LIST.rectbox='circle';
    MAP_VAR_LIST.uradius=90;
    MAP_VAR_LIST.rotang=0;
    MAP_VAR_LIST.ellipsoid='normal';
    k=2;
    while k<length(varargin)
      switch varargin{k}(1:3)
         case 'lon'
           MAP_VAR_LIST.ulong=varargin{k+1}(:)';
         case 'lat'
           MAP_VAR_LIST.ulat=varargin{k+1}(:)';
         case {'rad','alt'}
           MAP_VAR_LIST.uradius=varargin{k+1};
         case 'rec'
           MAP_VAR_LIST.rectbox=varargin{k+1};
	 case 'rot'
	   MAP_VAR_LIST.rotang=varargin{k+1};
         otherwise
           disp(['Unknown option: ' varargin{k}]);
      end
      k=k+2;
    end
    if strcmp(MAP_VAR_LIST.rectbox,'off'), MAP_VAR_LIST.rectbox='circle'; end

    MAP_VAR_LIST.rlong=MAP_VAR_LIST.ulong*pi180;
    MAP_VAR_LIST.rlat=MAP_VAR_LIST.ulat*pi180;

    % Compute the various limits
    % For the perspective viewpoints, we can't go *quite* to the horizon because this causes
    % problems in the clipping later on - but we can get pretty close (within .995) of it.
    % This is a fudge factor that can probably be changed if we increase the number of points
    % in grid lines...

    if length(MAP_VAR_LIST.uradius)==1
      if strcmp(MAP_PROJECTION.name,name{2}) && abs(MAP_VAR_LIST.uradius-90)<.2
        MAP_VAR_LIST.radius=89.8;
      elseif strcmp(MAP_PROJECTION.name,name{5})
        MAP_VAR_LIST.radius=min(80,MAP_VAR_LIST.uradius);
      elseif strcmp(MAP_PROJECTION.name,name{6})
        MAP_VAR_LIST.radius=acos(1/(1+MAP_VAR_LIST.uradius))/pi180*.98; % uradius is the height fraction here
      else
        MAP_VAR_LIST.radius=MAP_VAR_LIST.uradius;
      end
      rradius=MAP_VAR_LIST.radius*pi180;
    else
      % do some sperical trig
      edge=MAP_VAR_LIST.uradius*pi180 - [MAP_VAR_LIST.rlong 0];
      cosc=sin(MAP_VAR_LIST.rlat)*sin(edge(2))+cos(MAP_VAR_LIST.rlat)*cos(edge(2))*cos(edge(1));
      sinc=sqrt( ( cos(edge(2))*sin(edge(1)))^2 + ...
                 (cos(MAP_VAR_LIST.rlat)*sin(edge(2))-sin(MAP_VAR_LIST.rlat)*cos(edge(2))*cos(edge(1)))^2);
      rradius=atan2(sinc,cosc);
      MAP_VAR_LIST.radius=rradius/pi180;
    end
    MAP_VAR_LIST.cosradius=cos(rradius);

    switch MAP_PROJECTION.name
      case name(1)
        MAP_VAR_LIST.rhomax=2*tan(rradius/2);
      case name(2)
        MAP_VAR_LIST.rhomax=sin(rradius);
      case name(3)
        MAP_VAR_LIST.rhomax=2*sin(rradius/2);
      case name(4)
        MAP_VAR_LIST.rhomax=rradius;
      case name(5)
        MAP_VAR_LIST.rhomax=tan(rradius);
      case name(6)
        MAP_VAR_LIST.rhomax=sin(rradius)/(1+(1-cos(rradius))/MAP_VAR_LIST.uradius);
    end

    if strcmp(MAP_VAR_LIST.rectbox,'on')
      if length(MAP_VAR_LIST.uradius)==1
        MAP_VAR_LIST.xlims=[-1 1]/sqrt(2)*MAP_VAR_LIST.rhomax;
        MAP_VAR_LIST.ylims=[-1 1]/sqrt(2)*MAP_VAR_LIST.rhomax;
      else
        [X,Y]=mp_azim('ll2xy',MAP_VAR_LIST.uradius(1),MAP_VAR_LIST.uradius(2),'clip','off');
        MAP_VAR_LIST.xlims=[-abs(X) abs(X)];
        MAP_VAR_LIST.ylims=[-abs(Y) abs(Y)];
      end
    else
      MAP_VAR_LIST.xlims=[-MAP_VAR_LIST.rhomax MAP_VAR_LIST.rhomax];
      MAP_VAR_LIST.ylims=[-MAP_VAR_LIST.rhomax MAP_VAR_LIST.rhomax];
    end

    mu_util('lllimits');

  case 'll2xy'

    long=varargin{1}*pi180-MAP_VAR_LIST.rlong;
    lat=varargin{2}*pi180;
    vals=zeros(size(long));

    pi180=pi/180;
    cosc     =sin(MAP_VAR_LIST.rlat)*sin(lat)+cos(MAP_VAR_LIST.rlat)*(cos(lat).*cos(long));
    sinAzsinc=sin(long).*cos(lat);
    cosAzsinc=cos(MAP_VAR_LIST.rlat)*sin(lat)-sin(MAP_VAR_LIST.rlat)*(cos(lat).*cos(long));
    sinc=sqrt(sinAzsinc.^2+cosAzsinc.^2);

    switch MAP_PROJECTION.name
      case name(1)
        cosc(cosc==-1)=-1+eps;
        rho=2*sinc./(1+cosc);  % = 2*tan(c/2)
      case name(2)
        rho=sinc;   % = sinc
      case name(3)
        cosc(cosc==-1)=-1+eps;
        rho=sqrt(2)*sinc./sqrt(1+cosc);   % = 2*sin(c/2)
      case name(4)
        rho=atan2(sinc,cosc); % = c
      case name(5)
        rho=sinc./cosc; % = tan(c)
      case name(6)
        rho=sinc./(1+(1-cosc)/MAP_VAR_LIST.uradius); %
    end

    sinc(sinc==0)=eps;
    Az=(sinAzsinc+sqrt(-1)*cosAzsinc)./sinc;
    Az(abs(Az)==0)=-1;

    % Clip out-of-range values. We test against cos(c) (where c is the angular
    % distance from map center) rather than directly against rhomax, because
    % in the orthographic map rho->0 for points on the other side of the
    % globe whereas c does not!

    % Also, we clip on rho even if we later clip on X/Y because in some projections (e.g. the
    % orthographic) the X/Y locations wrap back.
    if ~strcmp(varargin{4},'off')
        vals = vals | cosc<=MAP_VAR_LIST.cosradius+eps*10;
        [rho,Az]=mu_util('clip',varargin{4},rho,MAP_VAR_LIST.rhomax,cosc<MAP_VAR_LIST.cosradius,Az);
        Az=Az./abs(Az);
    end

     X=rho.*real(Az*exp(i*pi180*MAP_VAR_LIST.rotang));
     Y=rho.*imag(Az*exp(i*pi180*MAP_VAR_LIST.rotang));

    if strcmp(MAP_VAR_LIST.rectbox,'on')  && ~strcmp(varargin{4},'off')
        vals= vals | X<=MAP_VAR_LIST.xlims(1)+eps*10 | X>=MAP_VAR_LIST.xlims(2)-eps*10 | ...
                     Y<=MAP_VAR_LIST.ylims(1)+eps*10 | Y>=MAP_VAR_LIST.ylims(2)-eps*10;
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(1),X<MAP_VAR_LIST.xlims(1) | isnan(X),Y);
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(2),X>MAP_VAR_LIST.xlims(2) | isnan(X),Y);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(1),Y<MAP_VAR_LIST.ylims(1) | isnan(Y),X);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(2),Y>MAP_VAR_LIST.ylims(2) | isnan(Y),X);
    end

  case 'xy2ll'

    rho=sqrt(varargin{1}.^2+varargin{2}.^2);
    Z=exp(i*(atan2(varargin{2},varargin{1})-MAP_VAR_LIST.rotang*pi180));
    V1=rho.*real(Z);
    V2=rho.*imag(Z);

    ir=rho==0;  % To prevent /0 warnings when rho is 0
    rho(ir)=eps;

    switch MAP_PROJECTION.name
      case name(1)
        c=2*atan(rho/2);
      case name(2)
        c=asin(rho);
        c(abs(rho)>1.0)=NaN;  % points outside the map
      case name(3)
        c=2*asin(rho/2);
         c(abs(rho)>2.0)=NaN;  % points outside the map
     case name(4)
        c=rho;
      case name(5)
        c=atan(rho);
      case name(6)
        arg1=(MAP_VAR_LIST.uradius+1)./sqrt(1+(MAP_VAR_LIST.uradius./rho).^2);
        c=asin(arg1) - atan(rho/MAP_VAR_LIST.uradius);
        c(arg1>1.0)=NaN;
    end
    c(ir)=eps; % we offset this slightly so that the correct limit is achieved in the
               % division below:

%    Y=(asin(cos(c)*sin(MAP_VAR_LIST.rlat) + ...
%            cos(MAP_VAR_LIST.rlat)*sin(c).*varargin{2}./rho))/pi180;
%
%    switch MAP_VAR_LIST.ulat,
%      case 90,
%        X=(MAP_VAR_LIST.rlong+atan2(varargin{1},-varargin{2}))/pi180;
%      case -90,
%        X=(MAP_VAR_LIST.rlong+atan2(varargin{1},varargin{2}))/pi180;
%      otherwise
%        X=(MAP_VAR_LIST.rlong+atan2( varargin{1}.*sin(c), ...
%          cos(MAP_VAR_LIST.rlat)*cos(c).*rho - sin(MAP_VAR_LIST.rlat)*varargin{2}.*sin(c) ) )/pi180;
%     end;

   % Can be problem if the argument is slightly larger than 1 - then the asin
   % returns a complex number.
   arg=cos(c)*sin(MAP_VAR_LIST.rlat) + ...
            cos(MAP_VAR_LIST.rlat)*sin(c).*V2./rho;
    arg=min(max(arg,-1),1);

    Y=(asin(arg))/pi180;

    switch MAP_VAR_LIST.ulat
      case 90
        X=(MAP_VAR_LIST.rlong+atan2(V1,-V2))/pi180;
      case -90
        X=(MAP_VAR_LIST.rlong+atan2(V1,V2))/pi180;
      otherwise
        X=(MAP_VAR_LIST.rlong+atan2( V1.*sin(c), ...
          cos(MAP_VAR_LIST.rlat)*cos(c).*rho - sin(MAP_VAR_LIST.rlat)*V2.*sin(c) ) )/pi180;
    end

  case 'xgrid'

    [X,Y,vals,labI]=mu_util('xgrid',MAP_VAR_LIST.longs,MAP_VAR_LIST.lats,varargin{1},31,varargin{2:3});

  case 'ygrid'

    [X,Y,vals,labI]=mu_util('ygrid',MAP_VAR_LIST.lats,MAP_VAR_LIST.longs,varargin{1},91,varargin{2:3});

  case 'box'

     [X,Y]=mu_util('box',31);

end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================

% function  [Jc,para,dJ,H] = PIRBFGSobjFctn(T,Rc,omega,m,beta,M,wRef,xc,wc)
%
% Objective Function for Parametric Image Registration with lBFGS
%
% computes J(wC) = D(T(Y(wc)),R) + S(wc), where
%
% yc       = y(wc,xc) = trafo(wc,xc)
% Tc       = T(yc) = inter(T,omega,yc),
% D(Tc,Rc) = distance(Tc,Rc,omega,m)
% S(wc)    = 0.5*(wc-wRef)'*M*(wc-wRef);
%
% Input:
%   T       coefficients for template image
%   Rc      sampled reference image
%   omega   spatial domain
%   m       number of discretization points
%   beta    adding beta*I to approximation of Hessian
%   xc      discretization of Omega
%   wc      current parameters
% Output:
%  Jc       current function value J(wc)
%  para     struct {Tc=T(y(wc)), Rc, omega, m, yc=y(wc,xc), Jc}, for plots
%  dJ       gradient of J
%  H        approximation to Hessian of J
%==============================================================================

function [Jc,para,dJ,H] = PIRBFGSobjFctn(T,Rc,omega,m,beta,M,wRef,xc,wc)

if nargin == 0,

  help(mfilename)
  runMinimalExample;
  Jc = 'endMinimalExample';
  return

elseif ~exist('wc','var') || isempty(wc),

  % if wc is not an input argument, reports status
  if nargout == 1, Jc = 'PIR';  return; end;

  % report current settings
  dimstr  = @(m) sprintf('[%s]',sprintf(' %d',m));
  wc      = trafo('w0');

  FAIRmessage('Parametric Image Registration with BFGS','-');
  fprintf('J(wc)=D(T(y(wc)),R) + (wc-wRef)''*M*(wc-wRef) != min\n');
  fprintf('%-20s : %s\n','m',dimstr(m));
  fprintf('%-20s : %s\n','omega',dimstr(omega));
  fprintf('%-20s : %s\n','IMAGE MODEL',imgModel);
  fprintf('%-20s : %s\n','DISTANCE',distance);
  fprintf('%-20s : %s\n','TRAFO',trafo);
  fprintf('%-20s : %s\n','length(wc)',num2str(length(wc)));
  fprintf('%-20s : %s\n','REGULARIZATION',...
    sprintf('M is %d-by-%d, beta=%s',size(M),num2str(beta)));
  FAIRmessage('-');

  return;
end;

% do the work ------------------------------------------------------------
doDerivative = (nargout>2);            % flag for necessity of derivatives

% compute transformation, distance, and regularization and combine these
[yc,dy]    = trafo(wc,center(xc,m),'doDerivative',doDerivative);
[Tc,dT]    = imgModel(T,omega,yc,'doDerivative',doDerivative);
[Dc,rc,dD] = distance(Tc,Rc,omega,m,'doDerivative',doDerivative);

% add regularization
if isempty(M) || (all(size(M)==1) && (M==0)),
  Sc = 0;
  dS = 0;
  M  = 0;
else
  dS = (wc-wRef)'*M;
  Sc = 0.5*dS*(wc-wRef);
end;

Jc = Dc + Sc;

% collect variables for plots
para = struct('Tc',Tc,'Rc',Rc,'omega',omega,'m',m,'yc',yc,'Jc',Jc);

if ~doDerivative, return; end;
dD = dD * dT;                    % multiply outer and inner derivatives
if size(dD,2) == size(dy,1),     % generic case, dy comes complete
  dJ = dD*dy + dS;
else                             % tricky case,  dy comes sparse
  n  = size(dy{1},1);
  dJ = reshape(dy{1}'*reshape(dD*dT,n,[]),1,[]) + dS;
%   dr = [dr(:,1:n)*dy{1},dr(:,n+1:2*n)*dy{1},dr(:,2*n+1:3*n)*dy{1}];
end;
if nargout<4, return; end;

% approximation to Hessian
if M == 0,
  H = 1;
else
  H = M;
end;

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

function runMinimalExample

setup2DhandData;

% extract data of level 4
level = 4;
omega = ML{level}.omega;
m     = ML{level}.m;

imgModel('reset','imgModel','linearInter');
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega);
xc    = getCellCenteredGrid(omega,m);
Rc    = imgModel(R,omega,xc);

% initialize distance measure
distance('reset','distance','SSD');

% initialize the transformation and a starting guess
% trafo('reset','trafo','affine3Dsparse');
trafo('reset','trafo','affine2D');
w0 = trafo('w0');

%------------------------------------------------------------------------------
% build objective function
% note: T  is data for template image
%       Rc is sampled reference image
%       optional Tikhonov-regularization is disabled by setting m = [], wRef = []
%       beta = 0, M = [], wRef = []:
%       disables additional regularization of Hessian approximation
beta = 0; M = []; wRef = [];
fctn = @(wc) PIRobjFctn(T,Rc,omega,m,beta,M,wRef,xc,wc);
fctn([]);   % report status
checkDerivative(fctn,w0+rand(size(w0)));

% setup plots and initialize
FAIRplots('reset','mode','PIR-objective','omega',omega,'m',m,'fig',1,'plots',1);
FAIRplots('init',struct('Tc',T,'Rc',R,'omega',omega,'m',m));

%% -- solve the optimization problem on one level
OPTpara = FAIRcell2struct(optPara('lBFGS','solver',''));
[wc,his] = lBFGS(fctn,w0,OPTpara{:});
return;

% %------------------------------------------------------------------------------
% %% finally: run the MultiLevel Non-Parametric Image Registration
% [wc,his] = MLPIR(ML);
%==============================================================================


function Y = as_l(tr,X)
%
%  Solves linear systems with the real, symmetric, negative definite matrix A,
%  i.e., Y = inv(A)*X.
%
%  The Cholesky factor of -A is provided as global data. This data must
%  be generated by calling 'as_l_i' before calling this routine!
%
%  Calling sequence:
%
%    Y = as_l(tr,X)
%
%  Input:
%
%    tr        is not referenced;
%    X         matrix of proper size.
%
%  Output:
%
%    Y         the solution matrix.
%
%
%   LYAPACK 1.0 (Thilo Penzl, May 1999)

if nargin~=2
  error('Wrong number of input arguments.');
end

global LP_U

if ~length(LP_U)
  error('This routine needs global data which must be generated by calling ''as_l_i'' first.');
end

Y = -LP_U\(LP_U'\X);      % Note the minus!


function [C] = slerp(A,B,t)
  % SLERP Spherically interpolate between to unit vectors. If not unit then
  % this will normalize, slerp and then lerp magnitudes.
  %
  % [C] = slerp(A,B,t)
  %
  % Inputs:
  %   A  #A by 3 list of unit vectors
  %   B  #A by 3 list of unit vectors
  %   t  #t list of values between 0 and 1
  % Outputs:
  %   C  #A by 3 list of interpolated unit vectors
  %

  assert(size(A,1)==size(B,1),'#A != #B');
  if numel(t) == 1
    t = repmat(t,size(A,1),1);
  end
  assert(size(A,1)==numel(t),'#A != #T');

  % Original magnitudes
  Amag = sqrt(sum(A.^2,2));
  Bmag = sqrt(sum(B.^2,2));
  % normalize a and b and keep 0's
  A = bsxfun(@rdivide,A,Amag);
  B = bsxfun(@rdivide,B,Bmag);
  Omega = acos( sum(A.*B,2) );
  Omega(abs(Omega-pi)<1e-8) = pi-10*1e-8;
  sinOmega = sin(Omega);
  C = bsxfun(@times,A,sin((1-t).*Omega)./sinOmega) + ...
      bsxfun(@times,B,sin(t.*Omega)./sinOmega);
  % coincident
  co = abs(Omega)<eps;
  % Converge to typical lerp
  if any(co)
    C(co,:) = A(co,:) + bsxfun(@times,t(co),(B(co,:)-A(co,:)));
  end

  % lerp original magnitudes
  C = bsxfun(@times,C,Amag + t.*(Bmag-Amag));

end


function [tsave, xsave] = quadSim(traj_obj, model_param, KK, t_sim, x0, Fmat, options)

[tsave, xsave] = ode45(@(t,s) quadDynamics(t, s, traj_obj, model_param, KK, Fmat), t_sim, x0, options);

desired_pos = zeros(length(tsave),3);
desired_vel = zeros(length(tsave),3);
desired_acc = zeros(length(tsave),3);

for i = 1:length(tsave)
    desired_s = desiredState(traj_obj, tsave(i));
    desired_pos(i,:) = desired_s.pos';
    desired_vel(i,:) = desired_s.vel';
    desired_acc(i,:) = desired_s.acc';
end

% position,attitude error plot
figure(2)
subplot(4,1,1)
plot(tsave,xsave(:,1)-desired_pos(:,1),'-b','LineWidth',1.0);title('Position error, x-x_d (m)');ylabel('x')
grid on
subplot(4,1,2)
plot(tsave,xsave(:,2)-desired_pos(:,2),'-b','LineWidth',1.0);ylabel('y')
grid on
subplot(4,1,3)
plot(tsave,xsave(:,3)-desired_pos(:,3),'-b','LineWidth',1.0);ylabel('z');%xlabel('time, sec')
grid on
% Orientation error plot (using error function)
yawd = desired_s.yaw;
yawd_dot = desired_s.yawdot;
yawd_2dot = desired_s.yawddot;
xcd = [cos(yawd) sin(yawd) 0]';
zbd = zeros(length(tsave),3);
ybd = zeros(length(tsave),3);
so3_error = zeros(length(tsave),1);
for i = 1:length(tsave)
    des_t = desired_acc(i,:) + [0 0 model_param.grav];
    zbd(i,:) = des_t/norm(des_t);
    ybd(i,:) = (hat_optr(zbd(i,:))*xcd/norm(hat_optr(zbd(i,:))*xcd))';
    xbd = hat_optr(ybd(i,:))*zbd(i,:)';
    Rd = [xbd ybd(i,:)' zbd(i,:)'];
    R = ROTZ(xsave(i,9))*ROTX(xsave(i,7))*ROTY(xsave(i,8));
    so3_error(i) = error_so3(R,Rd);
end
figure(2)
subplot(4,1,4)
plot(tsave,so3_error,'-b','LineWidth',1.0);title('Attitude error function, \Psi');xlabel('time, sec')
grid on
hold off
% Input plot
calc_acc = diff(xsave(:,4:6))./diff(tsave);
calc_ang_acc = diff(xsave(:,10:12))./diff(tsave);
t_vec = calc_acc;
t_vec(:,3) = t_vec(:,3)+model_param.grav;
norm_t = zeros(size(t_vec,1),1);
M = zeros(size(t_vec,1),3);
for i=1:length(norm_t)
    norm_t(i) = norm(t_vec(i,:));
    M(i,:) = model_param.I*calc_ang_acc(i,:)' + hat_optr(xsave(i,10:12))*model_param.I*xsave(i,10:12)';
end
u1 = model_param.mass*norm_t;

% plot for total thrust and moment
% figure(3)
% subplot(4,1,1)
% plot(tsave(1:end-1)+diff(tsave)/2,u1)
% subplot(4,1,2)
% plot(tsave(1:end-1)+diff(tsave)/2,M(:,1))
% subplot(4,1,3)
% plot(tsave(1:end-1)+diff(tsave)/2,M(:,2))
% subplot(4,1,4)
% plot(tsave(1:end-1)+diff(tsave)/2,M(:,3))
U = [u1 M];
L = model_param.arm_length;
c_tf = model_param.c_tf;
mapping_u = [1 1 1 1;0 L 0 -L;-L 0 L 0;c_tf -c_tf c_tf -c_tf];
motor_F = zeros(size(U));
for i=1:length(U)
    trans_F = mapping_u\U(i,:)';
    motor_F(i,:) = trans_F';
end
% plot for thrust of each rotor
figure(3)
subplot(4,1,1)
plot(tsave(1:end-1)+diff(tsave)/2,motor_F(:,1),'-b','LineWidth',1.0);title('Thrust of each rotor, (N)')
grid on
subplot(4,1,2)
plot(tsave(1:end-1)+diff(tsave)/2,motor_F(:,2),'-b','LineWidth',1.0)
grid on
subplot(4,1,3)
plot(tsave(1:end-1)+diff(tsave)/2,motor_F(:,3),'-b','LineWidth',1.0)
grid on
subplot(4,1,4)
plot(tsave(1:end-1)+diff(tsave)/2,motor_F(:,4),'-b','LineWidth',1.0);xlabel('time, sec')
grid on
hold off
end


function value = r8_chu ( a, b, x )

%*****************************************************************************80
%
%% R8_CHU evaluates the confluent hypergeometric function of R8 arguments.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 October 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real A, B, the parameters.
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the function value.
%
  persistent eps

  if ( isempty ( eps ) )
    eps = r8_mach ( 3 );
  end

  if ( x < 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_CHU - Fatal error!\n' );
    fprintf ( 1, '  X < 0.\n' );
    error ( 'R8_CHU - Fatal error!' )
  end

  if ( x == 0.0 )
    if ( 1.0 <= b )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8_CHU - Fatal error!\n' );
      fprintf ( 1, '  X = 0 and 1 <= B.\n' );
      error ( 'R8_CHU - Fatal error!' )
    end
    value = r8_gamma ( 1.0 - b ) / r8_gamma ( 1.0 + a - b );
    return
  end

  if ( max ( abs ( a ), 1.0 ) * max ( abs ( 1.0 + a - b ), 1.0 ) ...
    < 0.99 * abs ( x ) )
    value = x^( - a ) * r8_chu_scaled ( a, b, x );
    return
  end
%
%  The ascending series will be used, because the descending rational
%  approximation (which is based on the asymptotic series) is unstable.
%
  if ( 0.0 <= b )
    aintb = r8_aint ( b + 0.5 );
  else
    aintb = r8_aint ( b - 0.5 );
  end
  beps = b - aintb;
  n = aintb;
  alnx = log ( x );
  xtoeps = exp ( - beps * alnx );
%
%  Evaluate the finite sum.
%
%  Consider the case b < 1.0 first.
%
  if ( n < 1 )

    sum = 1.0;

    t = 1.0;
    m = - n;
    for i = 1 : m
      xi1 = i - 1;
      t = t * ( a + xi1 ) * x / ( ( b + xi1 ) * ( xi1 + 1.0 ) );
      sum = sum + t;
    end

    sum = r8_poch ( 1.0 + a - b, - a ) * sum
%
%  Now consider the case 1 <= b.
%
  else

    sum = 0.0;
    m = n - 2;

    if ( 0 <= m )

      t = 1.0;
      sum = 1.0;

      for i = 1 : m
       xi = i;
        t = t * ( a - b + xi ) * x / ( ( 1.0 - b + xi ) * xi );
        sum = sum + t;
      end

      sum = r8_gamma ( b - 1.0 ) * r8_gamr ( a ) * x^( 1 - n ) * xtoeps * sum;

    end

  end
%
%  Next evaluate the infinite sum.
%
  if ( n < 1 )
    istrt = 1 - n;
  else
    istrt = 0;
  end

  xi = istrt;

  factor = r8_mop ( n ) * r8_gamr ( 1.0 + a - b ) * x^istrt;

  if ( beps ~= 0.0 )
    factor = factor * beps * pi / sin ( beps * pi );
  end

  pochai = r8_poch ( a, xi );
  gamri1 = r8_gamr ( xi + 1.0 );
  gamrni = r8_gamr ( aintb + xi );
  b0 = factor * r8_poch ( a, xi - beps ) * gamrni ...
    * r8_gamr ( xi + 1.0 - beps );
%
%  x^(-beps) is close to 1.0, so we must be careful in evaluating
%  the differences.
%
  if ( abs ( xtoeps - 1.0 ) <= 0.5 )

    pch1ai = r8_poch1 ( a + xi, - beps );
    pch1i = r8_poch1 ( xi + 1.0 - beps, beps );
    c0 = factor * pochai * gamrni * gamri1 * ( ...
      - r8_poch1 ( b + xi,- beps ) + pch1ai ...
      - pch1i + beps * pch1ai * pch1i );
%
%  xeps1 = (1.0 - x^(-beps))/beps = (x^(-beps) - 1.0)/(-beps)
%
    xeps1 = alnx * r8_exprel ( - beps * alnx );

    value = sum + c0 + xeps1 * b0;
    xn = n;

    for i = 1 : 1000
      xi = istrt + i;
      xi1 = istrt + i - 1;
      b0 = ( a + xi1 - beps ) * b0 * x ...
        / ( ( xn + xi1 ) * ( xi - beps ) );
      c0 = ( a + xi1 ) * c0 * x / ( ( b + xi1) * xi ) ...
        - ( ( a - 1.0 ) * ( xn + 2.0 * xi - 1.0 ) ...
        + xi * ( xi - beps ) ) * b0 ...
        / ( xi * ( b + xi1 ) * ( a + xi1 - beps ) );
      t = c0 + xeps1 * b0;
      value = value + t;
      if ( abs ( t ) < eps * abs ( value ) )
        return
      end
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_CHU - Fatal error!\n' );
    fprintf ( 1, '  No convergence in 1000 terms.\n' );
    error ( 'R8_CHU - Fatal error!' )

  end
%
%  x^(-beps) is very different from 1.0, so the straightforward
%  formulation is stable.
%
  a0 = factor * pochai * r8_gamr ( b + xi ) * gamri1 / beps;
  b0 = xtoeps * b0 / beps;

  value = sum + a0 - b0;

  for i = 1 : 1000
    xi = istrt + i;
    xi1 = istrt + i - 1;
    a0 = ( a + xi1 ) * a0 * x / ( ( b + xi1 ) * xi );
    b0 = ( a + xi1 - beps ) * b0 * x ...
      / ( ( aintb + xi1 ) * ( xi - beps ) );
    t = a0 - b0;
    value = value + t;
    if ( abs ( t ) < eps * abs ( value ) )
      return
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8_CHU - Fatal error!\n' );
  fprintf ( 1, '  No convergence in 1000 terms.\n' );
  error ( 'R8_CHU - Fatal error!' )

end


function [types, isSecondary] = classifyMoieties(L, S)
% Classifies conserved moieties for a metabolic network
%
% USAGE:
%
%    types = classifyMoieties(L, S)
%
% INPUTS:
%    L:        The `r` x `m` moiety matrix with moiety vectors as columns.
%    S:        The `m` x `n` total stoichiometric matrix.
%
% OUTPUT:
%    types:    an `r` x `1` cell array of with one of the following moiety classifications
%              'Internal' moiety that is also conserved in the open network
%              'Transitive' moiety that is only found in primary metabolites
%              'Integrative' moiety that is not conserved in the open
%               network and found in both primary and secondary metabolites.
%
%
% isSecondary  `m x 1` Boolean vector indicating secondary metabolites
%                      (containing at least one internal conserved moiety')
%
% .. Author: - Hulda S. Haraldsdóttir, June 2015
%              Ronan Fleming, Oct 2020

types = cell(size(L,1),1);

isInternal = ~any(L*S,2); % Internal moieties are conserved in the open network
isSecondary = any(L(isInternal,:),1); % Secondary metabolites contain internal moieties
isTransitive = ~any(L(:,isSecondary),2); % Transitive moieties are only found in primary metabolites
isIntegrative = ~(isTransitive | isInternal); % All other moieties are Integrative

types(isTransitive) = {'Transitive'};
types(isIntegrative) = {'Integrative'};
types(isInternal) = {'Internal'};


function out = cwpt2_interface(in, direction, filter_type, trans_type, btree)
% function out = cwpt2_interface(in, direction, filter_type, trans_type, btree)
%
% Forward 2D Continuous Wavelet Transform.
% Inverse 2D Continuous Wavelet Transform.
% Shifted Inverse 2D Continuous Wavelet Transform.
%
% Forward 2D Continuous Wavelet Packet Transform.
% Inverse 2D Continuous Wavelet Packet Transform.
%
% This is a wrapper for cwpt2, cwpt2i, c3wpt, c3wpti.
% It allows to select arbitrary filters to be used for the transform.
% It allows classical (quad) or 3-way wavelet elementary transforms.
%
% direction =   'forward' | 'inverse'
% filter_type = '7-9' | 'spline'
% trans_type =  'quad' | 'tri'
% btree =       a) for wavelet packet transforms, the corresponding btree
%               b) for forward wavelet transform, the number of scales
%               c) for inverse wavelet transform, must bet set to 0
%               d) for shifted inverse wavelet transform, must be set to 1
%
% A forward transform given a M x N image provides a M x N x S 3-D matrix.
%
% ================
% For Wavelet Packet Transform, type 'help cwpt2' for more information about the
% order of subbands.
%
% ================
% For Wavelet Transform, S = ( 1 + 3 * nb_scales ) and the order of subbands is:
%
% HL, LH, HH (scale 1),
% HL, LH, HH (scale 2),
% ...
% LL, HL, LH, HH (scale nb_scales),
%
% where the first filtering direction is the first memory raster direction of data.
% For instance, LH means for Matlab images low-filtering along columns (first dimension)
% and high-filtering along rows (second dimension).
%
% ================
% For 3-way Wavelet Transform, S = ( 1 + 2 * nb_scales ) and the order of subbands is:
%
% H0, 0H (scale 1),
% H0, 0H (scale 2),
% ...
% LL, H0, 0H (scale nb_scales),
%
% ================
% A shifted inverse (classical or 3-way) wavelet transform considers a transform of
% scale N as a transform of scale (N+1)
%

if ~isequal(class(in),'double')
    warning('Input value should be of type ''double''.');
    in = double(in);
end;

switch lower(filter_type)
case '7-9'
    % analysis
    alf = [ 0.03782845550699547033,-0.02384946501937996663,-0.1106244044184234859, 0.3774028556126538536, 0.8526986790094033264, 0.3774028556126538536,-0.1106244044184233194,-0.02384946501937999092, 0.03782845550699546339];
    ahf = [ 0.06453888262893847649,-0.04068941760955852721,-0.4180922732222120963, 0.7884856164056645023,-0.4180922732222124294,-0.04068941760955835374, 0.06453888262893842098];
    % reconstruction
    rlf = [-0.03226944131446923825,-0.02034470880477926361, 0.2090461366111060482, 0.3942428082028322511, 0.2090461366111062147,-0.02034470880477917687,-0.03226944131446921049];
    % ahf / 2; * -1 ^ odd
    rhf = [ 0.01891422775349773516, 0.01192473250968998331,-0.05531220220921174296,-0.1887014278063269268, 0.4263493395047016632,-0.1887014278063269268,-0.0553122022092116597, 0.01192473250968999546, 0.01891422775349773169];
    % alf / 2; * -1 ^ even
case 'spline'
    h = MakeONFilter_copy('Battle',3);
    g = ((-1 * ones(1,41)).^(0:40)).*h;

    alf = h;
    ahf = g;
    rlf = h./2;
    rhf = g./2;
otherwise
    error('Unknown filter type');
end;

switch lower(direction)
case 'forward'

    if length(btree)==1                         % wavelet; btree = nb_scales
        btree = cwpt2_btree(btree, 2);          % 2 = wavelet
    else                                        % packets
        if isequal(trans_type,'tri')
            warning('3-way transform has not been tested for WP transform');
        end;
    end;

    switch lower(trans_type)
    case 'quad'
        out = cwpt2(in, btree, alf, floor(length(alf)/2), ahf, floor(length(ahf)/2));
    case 'tri'
        out = c3wpt(in, btree, alf, floor(length(alf)/2), ahf, floor(length(ahf)/2));
    otherwise
        error('Unknown transform type');
    end;

case 'inverse'

    if length(size(in))~=3
        error('The input argument must be a M x N x S 3-D matrix.');
    end;

    if length(btree)==1                         % wavelet; btree = scale_shift
        if btree~=0 && btree~=1
            error('btree parameter must be 0 or 1 for inverse wavelet transform')
        end;

        nb_scales = size(in,3) - 1;
        if isequal(trans_type,'quad')
            if mod(nb_scales,3)~=0
                error('Invalid data: the size along the dimension 3 must be ( 1 + 3 * nb_scales ).');
            end;
            nb_scales = btree + (nb_scales / 3);
        else
            if mod(nb_scales,2)~=0
                error('Invalid data: the size along the dimension 3 must be ( 1 + 2 * nb_scales ).');
            end;
            nb_scales = btree + (nb_scales / 2);
        end;

        btree = cwpt2_btree(nb_scales, 2);      % 2 = wavelet
    else                                        % packets
        if isequal(trans_type,'tri')
            warning('3-way transform has not been tested for WP transform');
        end;
    end;

    switch lower(trans_type)
    case 'quad'
        out = cwpt2i(in, btree, rlf, floor(length(rlf)/2), rhf, floor(length(rhf)/2));
    case 'tri'
        out = c3wpti(in, btree, alf, floor(length(alf)/2), ahf, floor(length(ahf)/2), rlf, floor(length(rlf)/2), rhf, floor(length(rhf)/2));
    otherwise
        error('Unknown transform type');
    end;

otherwise
    error('Incorrect transform direction');
end;

% if the output argument is a string, this is an error message
if isequal(class(out),'char')
    error(out);
end;

% 2D Continuous Wavelet Packet Transform package
% (c) 2002-2005 Let It Wave, all rights reserved


function p13_demo ( iteration_max, h )

%*****************************************************************************80
%
%% P13_DEMO runs the 2D demo problem #13, with mesh size H.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 February 2006
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer ITERATION_MAX, the maximum number of iterations that DISTMESH
%    should take.  (The program might take fewer iterations if it detects convergence.)
%
%    Input, real H, the mesh spacing parameter.
%
  if ( nargin < 1 )
    iteration_max = 200;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P13_DEMO - Note:\n' );
    fprintf ( 1, '  No value of ITERATION_MAX was supplied.\n' );
    fprintf ( 1, '  The default value ITERATION_MAX = %d will be used.\n', ...
      iteration_max );
  end

  if ( nargin < 2 )
    h = 0.025;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P13_DEMO - Note:\n' );
    fprintf ( 1, '  No value of H was supplied.\n' );
    fprintf ( 1, '  The default value H = %f will be used.\n', h );
  end
%
%  Put the random number generator into a fixed initial state.
%
  rand ( 'state', 111 );
%
%  Set the rendering method for the current figure to Z-buffering.
%
  set ( gcf, 'rend', 'z' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Problem 13:\n' );
  fprintf ( 1, '  The Sandia Fork, h = %f\n', h )

  fd = @p13_fd;
  fh = @p13_fh;
  box = [0.0, 0.0; 1.0, 1.0];

  fixed = [ ...
   0.10000, 0.00000; ...
   0.20000, 0.00000; ...
   0.80000, 0.00000; ...
   0.90000, 0.00000; ...
   0.55000, 0.39686; ...
   0.55000, 0.90000; ...
   0.45000, 0.90000; ...
   0.45000, 0.39686 ];

  [ p, t ] = distmesh_2d ( fd, fh, h, box, iteration_max, fixed );

  post_2d ( p, t, fh )
%
%  Write a PostScript image of the triangulation.
%
  [ node_num, junk ] = size ( p );
  [ tri_num, junk ] = size ( t );
  p = p';
  t = t';
  node_show = 0;
  triangle_show = 1;

  triangulation_order3_plot ( 'p13_mesh.eps', node_num, p, tri_num, ...
    t, node_show, triangle_show );
%
%  Write a text file containing the nodes.
%
  r8mat_write ( 'p13_nodes.txt', 2, node_num, p );
%
%  Write a text file containing the triangles.
%
  i4mat_write ( 'p13_elements.txt', 3, tri_num, t );

  return
end


 function G = Gnearest(nx, ny, nb, na, mask)
%|function G = Gnearest(nx, ny, nb, na, mask)
%|
%| Generate a geometric system matrix for tomographic projection
%| based on simple nearest-neighbor "interpolation."
%| This system model is completely inadequate for reconstructing
%| real tomographic data, but is marginally useful for simple simulations.
%|
%| The image size is nx * ny.
%| The sinogram size is nb * na (n_radial_bins X n_angles).
%| Returned G is [nb * na, nx * ny] - this size is need for .wtf saving.
%|
%| Jeff Fessler

if nargin < 1, nx = 64; end
if nargin < 2, ny = nx; end
if nargin < 3, nb = nx; end
if nargin < 4, na = floor(nb * pi/2); end
if nargin < 5, mask = true(nx,ny); end

warning 'this function is obsolete.  use Gtomo2_strip() instead'

% pixel centers
[x,y] = ndgrid([0:nx-1] - (nx-1)/2, [0:ny-1] - (ny-1)/2);
x = x(mask(:));
y = y(mask(:));
np = length(x);		% sum(mask(:)) - total # of support pixels

angle = [0:na-1]'/na * pi;
tau = cos(angle) * x' + sin(angle) * y';	% [na,np] projected pixel center
ii = round(tau + (nb-1)/2);			% counting from 1 (matlab)
good = ii(:) >= 1 & ii(:) <= nb;
if any(~good), warning 'FOV too small', end

ii = ii + [0:na-1]' * nb * ones(1,np);

%np = sum(mask(:));
%nc = np;	jj = 1:np;		% compact G
nc = nx * ny;	jj = find(mask(:))';	% all-column G
jj = jj(ones(1,na),:);

G = sparse(ii(good), jj(good), ones(sum(good),1), nb*na, nc);

if 0
%	subplot(121), im(embed(sum(G)', mask))		% for compact
	subplot(121), im(reshape(sum(G), nx, ny))	% for all-column
	subplot(122), im(reshape(sum(G'), nb, na))
end


classdef HaloOrbitApproximator < matlab.mixin.SetGet
    %HaloOrbitApproximator Summary of this class goes here
    %   Detailed explanation goes here

    properties
        gm1(1,1) double
        gm2(1,1) double
        sma2(1,1) double
        radius1(1,1) double
        radius2(1,1) double
        AzUnscaled(1,1) double
        LPt(1,1) string = "L1"
        side(1,1) string = "northern"

        %diff corr options
        diffCorAlpha(1,1) double = 0.01;
        adjustIndEnum(1,1) HaloCoordAdjustEnum = HaloCoordAdjustEnum.Z;
    end

    properties(Access=public, Dependent)
        T2(1,1) double
        LU(1,1) double
        TU(1,1) double
        VU(1,1) double
        mu(1,1) double
        gL(1,1) double
        AzScaled(1,1) double
    end

    properties(Transient)
        corrSol
        manifoldLine(1,1)
        rVectCR3BP_Prim_Manifold(3,1) double
        vVectCR3BP_Prim_Manifold(3,1) double
    end

    events
        UpdateCalcWaitbar
    end

    methods
        function obj = HaloOrbitApproximator(gm1, gm2, sma2, radius1, radius2, AzUnscaled, LPt, side)
            obj.gm1 = gm1;
            obj.gm2 = gm2;
            obj.sma2 = sma2;
            obj.radius1 = radius1;
            obj.radius2 = radius2;
            obj.AzUnscaled = AzUnscaled;
            obj.LPt = string(upper(LPt));
            obj.side = string(lower(side));
        end

        function val = getT2(obj)
            val = computePeriod(obj.sma2, obj.gm1);
        end

        function val = get.LU(obj)
            val = obj.sma2;
        end

        function val = get.TU(obj)
            val = 1/sqrt((obj.gm1 + obj.gm2)/obj.sma2^3);
        end

        function val = get.VU(obj)
            val = obj.LU / obj.TU;
        end

        function val = get.mu(obj)
            val = obj.gm2/(obj.gm1 + obj.gm2);
        end

        function val = get.gL(obj)
            LP = HaloOrbitApproximator.lagrangePoints(obj.mu);

            switch obj.LPt
                case "L1"
                    val = abs(1 - obj.mu - LP(1,1));
                case "L2"
                    val = abs(LP(2,1) - 1 + obj.mu);
                otherwise
                    error('Unknown Lagrange point.');
            end
        end

        function val = get.AzScaled(obj)
            val = (obj.AzUnscaled/obj.LU)/obj.gL;
        end

        function computeManifold(obj, tFrac, whichManifold, propDurDimensioned, hAx)
            sol = obj.corrSol;
            if(not(isempty(sol)))
                T = sol.x';
                Y = sol.y';

                stateF_Monodromy = reshape(Y(end,7:end),6,6);
                [V,D] = eig(stateF_Monodromy);
                eigvalues = diag(D,0);
                [~, IMaxEig] = max(real(eigvalues));
                [~, IMinEig] = min(real(eigvalues));

                Vs = V(:, IMinEig);
                Vu = V(:, IMaxEig);

                tFracActual = tFrac * (T(end) - T(1));
                Ynd_corr_Tfrac = deval(sol,tFracActual)';
                stateF_STM = reshape(Ynd_corr_Tfrac(1,7:end),6,6);

                Vs_i = stateF_STM * Vs;
                Vu_i = stateF_STM * Vu;

                epsilson = [0; 0; 0; 1E-4; 1E-4; 1E-4];
                Xi = Ynd_corr_Tfrac(1,1:6);
                XiS = Xi(:) + epsilson .* normVector(Vs_i);
                XiU = Xi(:) + epsilson .* normVector(Vu_i);

                propDur = propDurDimensioned / obj.TU;
                if(propDur > 0)
                    switch whichManifold
                        case HaloArriveDepartManifoldEnum.Arrive
                            [T_manifold, Y_manifold] = obj.simulateOrbit(XiU(:)', propDur, false);

                        case HaloArriveDepartManifoldEnum.Depart
                            [T_manifold, Y_manifold] = obj.simulateOrbit(XiS(:)', -propDur, false);

                        otherwise
                            error('Unknown manifold type.');
                    end

                else
                    T_manifold = 0;
                    Y_manifold = XiS(:)';
                end

                Y_manifold_Pos_Dim = Y_manifold(:,1:3) * obj.LU;
                if(isempty(obj.manifoldLine) || not(isgraphics(obj.manifoldLine, 'Line')))
                    obj.manifoldLine = plot3(hAx, Y_manifold_Pos_Dim(:,1), Y_manifold_Pos_Dim(:,2), Y_manifold_Pos_Dim(:,3));
                    obj.manifoldLine.DisplayName = 'Arrival/Departure Orbit';
                else
                    obj.manifoldLine.XData = Y_manifold_Pos_Dim(:,1);
                    obj.manifoldLine.YData = Y_manifold_Pos_Dim(:,2);
                    obj.manifoldLine.ZData = Y_manifold_Pos_Dim(:,3);
                end

                primary = [-obj.mu, 0, 0];

                obj.rVectCR3BP_Prim_Manifold = (Y_manifold(1,1:3)-primary)' * obj.LU;
                obj.vVectCR3BP_Prim_Manifold = Y_manifold(1,4:6)' * obj.VU;
            end
        end

        function [rVectCR3BP_Prim, vVectCR3BP_Prim, period, approxAzAchieved, corrAzAchieved] = computeCorrectedInitialState(obj, hAx, dispPrimBody, dispSecBody)
            [~,~,~,~,~,~, statesND_approx, ~, Tapprox] = obj.getApproxHaloOrbitInitState(0);
            state0ND_diffCor = obj.differentialCorrector(statesND_approx(1,:), Tapprox);

            [Tnd_uncorr,Ynd_uncorr] = obj.simulateOrbit(statesND_approx(1,:), Tapprox, false);
            [Tnd_corr,Ynd_corr, ~,~,~, sol_corr] = obj.simulateOrbit(state0ND_diffCor, 2*Tapprox, false);
            obj.corrSol = sol_corr;

            approxAzAchieved = max(abs(statesND_approx(:,3))*obj.LU);
            corrAzAchieved = max(abs(Ynd_corr(:,3))*obj.LU);

            fprintf('Approx Az Achieved: %0.3f km\n', approxAzAchieved);
            fprintf('Corrected Az Achieved: %0.3f km\n', corrAzAchieved);

            if(isempty(hAx))
                hAx = axes(figure());
            else
                cla(hAx);
            end

            [hTrajApprox, hLPts] = obj.plotHaloOrbit(hAx, statesND_approx);
            [hTrajUnCorr, ~] = obj.plotHaloOrbit(hAx, Ynd_uncorr);
            [hTrajCorr,   ~] = obj.plotHaloOrbit(hAx, Ynd_corr);

            legendObjs = [hTrajApprox, hTrajUnCorr, hTrajCorr];

            hTrajApprox.DisplayName = 'Approx Halo';
            hTrajUnCorr.DisplayName = 'Uncorrected Halo';
            hTrajCorr.DisplayName   = 'Corrected Halo';
            hLPts.DisplayName = 'L Points';

            axis(hAx,'equal');
            hold(hAx,'on');

            %Plot secondary
            if(dispSecBody)
                dRad = obj.radius2;
                [X,Y,Z] = sphere(30);
                X = dRad*X + (1-obj.mu)*obj.LU;
                CData = getCDataForSphereWithColormap(Z, 'gray');
                legendObjs(end+1) = surf(hAx, X,dRad*Y,dRad*Z, 'CData',CData, 'BackFaceLighting','lit', 'FaceLighting','gouraud', 'EdgeLighting','gouraud', 'LineWidth',0.1, 'EdgeAlpha',0.1, 'DisplayName','Primary Body');
            end

            %Plot primary
            if(dispPrimBody)
                dRad = obj.radius1;
                [X,Y,Z] = sphere(30);
                X = dRad*X + (-obj.mu)*obj.LU;
                CData = getCDataForSphereWithColormap(Z, 'gray');
                legendObjs(end+1) = surf(hAx, X,dRad*Y,dRad*Z, 'CData',CData, 'BackFaceLighting','lit', 'FaceLighting','gouraud', 'EdgeLighting','gouraud', 'LineWidth',0.1, 'EdgeAlpha',0.1, 'DisplayName','Primary Body');
            end

%             {'Approx Halo', 'Uncorrected Halo', 'Corrected Halo', 'Primary Body', 'Secondary Body', 'L Points'};
            legend(legendObjs, 'Location','best');

            primary = [-obj.mu, 0, 0];

            rVectCR3BP_Prim = (Ynd_corr(1,1:3)-primary)' * obj.LU;
            vVectCR3BP_Prim = Ynd_corr(1,4:6)' * obj.VU;
            period = Tapprox * obj.TU;
        end

        function state0ND_approx = differentialCorrector(obj, state0ND_approx, Tapprox)
            MU = obj.mu;
            adjustInd = obj.adjustIndEnum.adjustInd;

            deltaXDot = Inf;
            deltaZDot = Inf;
            tol = 1E-12;
            while(abs(deltaXDot) > tol || ...
                    abs(deltaZDot) > tol)
                % 1. Propagate the initial state vector using an ODE solver, such as ode45 in Matlab,
                % until the position in y is equal to zero again (T/2).
                [Tnd_uncorr,Ynd_uncorr, te,ye,ie] = obj.simulateOrbit(state0ND_approx, 10*Tapprox, true);
                stateF = Ynd_uncorr(end,:);

                % 2. Find the error in x and z velocities at T/2 (x_ and z_).
                deltaXDot = -stateF(4);
                deltaZDot = -stateF(6);
                deltaVect = [deltaXDot;
                    deltaZDot];

                % 3. Calculate the change in the initial state required to reduce the error. It is only
                % necessary to change two of the initial states. Since all the zero terms are correct
                % those are left alone, and x and y_ are the only terms that will need to be altered.
                % The initial position in z will remain xed. x0 and y_0 are the values that will
                %be used to change the initial state.
                stateF_kinematic = stateF(1:6);
                stateF_STM = stateF(7:end);
                stateF_STM = reshape(stateF_STM,6,6);

                A = [stateF_STM(4,adjustInd), stateF_STM(4,5);
                    stateF_STM(6,adjustInd), stateF_STM(6,5)];

                b = [stateF_STM(2,adjustInd), stateF_STM(2,5)];

                accelVec = HaloOrbitApproximator.cr3bpODE(Tnd_uncorr(end),stateF_kinematic,MU);
                accelVec = [accelVec(4); accelVec(6)];

                vy = stateF_kinematic(5);

                RHS = (A - (1/vy)*accelVec*b);
                deltas = RHS \ deltaVect;
                deltas = deltas * obj.diffCorAlpha;

                % 4. Now, a new initial state is calculated by adding x0 and y_0 to the original
                %initial state as in Equation 3.59 and Equation 3.60.
                r0_new = state0ND_approx(adjustInd) + deltas(1);
                ydot0_new = state0ND_approx(5) + deltas(2);

                % 5. The new estimate for the initial state vector is now:
                state0ND_approx(adjustInd) = r0_new;
                state0ND_approx(5) = ydot0_new;

                curVal = log10(max(abs(deltaVect)));
                evtData = matlab.ui.eventdata.ValueChangedData(curVal, log10(tol));
                notify(obj, 'UpdateCalcWaitbar', evtData);
            end
        end

        function [x,y,z,xdot,ydot,zdot, statesND, statesDim, Tapprox] = getApproxHaloOrbitInitState(obj, phi0)
            [k, wp, nu, ...
                a21, a22, a23, a24, a31, a32, ...
                b21, b22, b31, b32, ...
                d21, d31, d32, Ax, Az] = obj.getRichardsonConstants();

            if(obj.side == "northern")
                m = 1;
            elseif(obj.side == "southern")
                m = 3;
            else
                error('Unknown side parameter');
            end

            Tapprox = (2*pi)/(wp*nu);
            tau = linspace(0,Tapprox,100);

            deltaM = 2 - m;
            tau1 = wp .* tau + phi0;

            g = obj.gL;

            x = g*(a21*Ax^2 + a22*Az - Ax*cos(tau1) + (a23*Ax^2 - a24*Az^2)*cos(2*tau1) + ...
                (a31*Ax^3 - a32*Ax*(Az^2)*cos(3*tau1)));

            y = g*(k*Ax*sin(tau1) + (b21*Ax^2 - b22*Az^2)*sin(2*tau1) + (b31*Ax^3 - b32*Ax*Az^2)*sin(3*tau1));

            z = g*(deltaM*Az*cos(tau1) + deltaM*d21*Ax*Az*(cos(2*tau1) - 3) + deltaM*(d32*Az*Ax^2 - d31*Az^3)*cos(3*tau1));

            xdot = g*(wp*nu*Ax*sin(tau1) - 2*wp*nu*(a23*Ax^2 - a24*Az^2)*sin(2*tau1) - 3*wp*nu*(a31*Ax^3 - a32*Ax*Az^2)*sin(3*tau1));

            ydot = g*(wp*nu*k*Ax*cos(tau1) + 2*wp*nu*(b21*Ax^2 - b22*Az^2)*cos(2*tau1) + 3*wp*nu*(b31*Ax^3 - b32*Ax*Az^2)*cos(3*tau1));

            zdot = g*(-wp*nu*deltaM*Az*sin(tau1) - 2*wp*nu*deltaM*d21*Ax*Az*sin(2*tau1) - 3*wp*nu*deltaM*(d32*Az*Ax^2 - d31*Az^2)*sin(3*tau1));

            LP = HaloOrbitApproximator.lagrangePoints(obj.mu);
            switch obj.LPt
                case "L1"
                    deltaX = LP(1,1);
                case "L2"
                    deltaX = LP(2,1);
                otherwise
                    error('Unknown Lagrange point.');
            end

            x = x + deltaX; %need to move the x-coords back to the barycenter for integration in the synodic frame

            statesND = [x(:), y(:), z(:), xdot(:), ydot(:), zdot(:)];
            statesDim = [x(:)*obj.LU, ...
                y(:)*obj.LU, ...
                z(:)*obj.LU, ...
                xdot(:)*obj.VU, ...
                ydot(:)*obj.VU, ...
                zdot(:)*obj.VU];
        end

        function [hTraj, hLPts] = plotHaloOrbit(obj, hAx, statesND)
            LP = HaloOrbitApproximator.lagrangePoints(obj.mu);
            LP = LP * obj.LU;

            statesDim = statesND * obj.LU;

            hold(hAx, 'on');
            hTraj = plot3(hAx, statesDim(:,1), statesDim(:,2), statesDim(:,3));
            grid(hAx, 'off');
            grid(hAx, 'minor');
            %             plot3(-obj.mu, 0, 0, 'ko');
            %             hSecBody = plot3(hAx, (1-obj.mu)*obj.LU, 0, 0, 'ko');
            %             hSecBody = obj.plotBody(hAx);
            hLPts = plot3(hAx, LP(1:2,1), LP(1:2,2), LP(1:2,3), 'bo');
            %             axis(hAx,'equal');
            hold(hAx, 'off');
            xlabel(hAx,'X [km]');
            ylabel(hAx,'Y [km]');
            zlabel(hAx,'Z [km]');
        end

        function hCBodySurf = plotBody(obj, hAx)
            dRad = obj.radius2 / obj.LU;
            [X,Y,Z] = sphere(50);

            CData = getCDataForSphereWithColormap(Z, 'gray');
            hCBodySurf = surf(hAx, dRad*X,dRad*Y,dRad*Z, 'CData',CData, 'BackFaceLighting','lit', 'FaceLighting','gouraud', 'EdgeLighting','gouraud', 'LineWidth',0.1, 'EdgeAlpha',0.1);
            material(hCBodySurf,'dull');
        end

        function [T,Y, te,ye,ie, sol] = simulateOrbit(obj, y0, tf, stopOnYCrossing)
            tspan=[0 tf];

            if(numel(y0) == 6)
                y0 = [y0(:)', reshape(eye(6),1,36)];
            end

            odefun = @(t,y) HaloOrbitApproximator.stateTransMatrixODE(t,y,obj.mu);

            evtFcn = @(t,y) HaloOrbitApproximator.odeEvents(t,y, stopOnYCrossing);
            options=odeset('RelTol',1e-12,'AbsTol',1e-12,'Events',evtFcn);

            sol = ode113(odefun, tspan, y0, options);

            T = sol.x';
            Y = sol.y';
            te = sol.xe';
            ye = sol.ye';
            ie = sol.ie';
        end
    end

    methods(Access=private)
        function f = correctedHaloOrbitObjFun(obj, x, state0ND_approx, Tapprox)
            xNew = x(1);
            zNew = x(2);
            ydotNew = x(3);
            state0ND_approx([1,3,5]) = [xNew, zNew, ydotNew];

            [~,Ynd, ~,~,~] = obj.simulateOrbit(state0ND_approx, 10*Tapprox, true);
            aZAchieved = max(abs(Ynd(:,3))*obj.LU);

            f = abs((aZAchieved - obj.AzUnscaled)/abs(obj.AzUnscaled));
        end

        function [c, ceq] = correctedHaloOrbitNonlcon(obj, x, state0ND_approx, Tapprox)
            xNew = x(1);
            zNew = x(2);
            ydotNew = x(3);
            state0ND_approx([1,3,5]) = [xNew, zNew, ydotNew];

            [~,Ynd, ~,~,~] = obj.simulateOrbit(state0ND_approx, 10*Tapprox, true);

            c = [];
            ceq(1) = Ynd(end,2); %y=0
            ceq(2) = Ynd(end,3); %xdot = 0
            ceq(3) = Ynd(end,5); %zdot = 0
        end

        function [k, wp, nu, ...
                a21, a22, a23, a24, a31, a32, ...
                b21, b22, b31, b32, ...
                d21, d31, d32, Ax, Az] = getRichardsonConstants(obj)

            %             c2 = (1/obj.gL^3) * (obj.mu + ((1-obj.mu)*obj.gL^3)/(1-obj.gL)^3);
            %             c3 = (1/obj.gL^3) * (obj.mu - ((1-obj.mu)*obj.gL^4)/(1-obj.gL)^4);
            %             c4 = (1/obj.gL^3) * (obj.mu + ((1-obj.mu)*obj.gL^5)/(1-obj.gL)^5);

            [~, c2, c3, c4] = obj.computeCCoeff();

            wp = sqrt(2 - c2 + ((9*c2^2 - 8*c2)/2)^(1/2));
            k = (wp^2 + 1 + 2*c2)/(2*wp);

            d1 = ((3*wp^2)/k) * (k*(6*wp^2-1)-2*wp);
            d2 = ((8*wp^2)/k) * (k*(11*wp^2-1)-2*wp);

            a21 = (3*c3*(k^2-2))/(4*(1+2*c2));
            a22 = (3*c3)/(4*(1+2*c2));
            a23 = ((-3*c3*wp)/(4*k*d1))*(3*(k^3)*wp - 6*k*(k-wp) + 4);
            a24 = ((-3*c3*wp)/(4*k*d1))*(2+3*k*wp);

            d21 = -c3/(2*wp^2);
            d31 = (3/(64*wp^2)) * (4*c3*a24 + c4);
            d32 = (3/(64*wp^2)) * (4*c3*(a23 - d21) + c4*(4+k^2));

            b21 = ((-3*c3*wp)/(2*d1))*(3*k*wp - 4);
            b22 = (3*c3*wp)/d1;
            b31 = (3/(8*d2)) * (8*wp*(3*c3*(k*b21 - 2*a23) - c4*(2+3*k^2)) + ...
                (9*wp^2 + 1 + 2*c2)*(4*c3*(k*a23-b21)+(k*c4*(4+k^2))));
            b32 = (1/d2) * (9*wp*(c3*(k*b22 + d21 - 2*a24) - c4) + ...
                (3/8)*(9*wp^2 + 1 + 2*c2)*(4*c3*(k*a24-b22)+(k*c4)));

            a31 = (-9*wp/(4*d2)) * (4*c3*(k*a23 - b21) + k*c4*(4+k^2)) + ...
                ((9*wp^2 + 1 - c2)/(2*d2))*(3*c3*(2*a23-k*b21)+(c4*(2+3*k^2)));
            a32 = (-1/d2) * ((9/4)*wp*(4*c3*(k*a24 - b22) + k*c4) + ...
                (3/2)*(9*wp^2 + 1 - c2)*(c3*(k*b22 + d21 - 2*a24) - c4));

            s1 = (1/(2*wp*(wp*(1+k^2)-2*k))) * ((3/2)*c3*(2*a21*(k^2-2) - a23*(k^2+2) - 2*k*b21) - ...
                (3/8)*c4*(3*k^4 - 8*k^2 + 8));
            s2 = (1/(2*wp*(wp*(1+k^2)-2*k))) * ((3/2)*c3*(2*a22*(k^2-2) + a24*(k^2+2) + 2*k*b22 + 5*d21) + ...
                (3/8)*c4*(12 - k^2));

            a1 = (-3/2)*c3*(2*a21 + a23 + 5*d21) - (3/8)*c4*(12-k^2);
            a2 = (3/2)*c3*(a24 - 2*a22) + (9/8)*c4;

            l1 = a1 + 2*(wp^2)*s1;
            l2 = a2 + 2*(wp^2)*s2;

            delta = wp^2 - c2;

            Az = obj.AzScaled;
            Ax = sqrt((-l2 * Az^2 - delta)/l1);

            nu = 1 + s1*Ax^2 + s2*Az^2;
        end

        function [c1, c2, c3, c4] = computeCCoeff(obj)
            g = obj.gL;
            m = obj.mu;

            switch obj.LPt
                case "L1"
                    Ceqn = @(n) (1/((g^3)*(1-g^(n+1)))) * (m + ((-1)^n)*(1 - m)*g^(n+1));
                case "L2"
                    Ceqn = @(n) (1/((g^3)*(1+g^(n+1)))) * (((-1)^n)*m + ((-1)^n)*(1 - m)*g^(n+1));
                otherwise
                    error('Unknown Lagrange point.');
            end

            c = NaN(4,1);
            for(n=1:4)
                c(n) = Ceqn(n);
            end

            c1 = c(1);
            c2 = c(2);
            c3 = c(3);
            c4 = c(4);
        end
    end

    methods(Static)
        function [value,isterminal,direction] = odeEvents(~,y, stopOnYCrossing)
            value = y(2); %when y=0
            direction = 0;

            if(stopOnYCrossing)
                isterminal = 1;
            else
                isterminal = 0;
            end
        end

        function LP = lagrangePoints(mu)
            %For a given value of mu, this function computes the location
            %of all five libration points for the circular restricted three
            %body problem.  It returns them as equilibrium points in R^3
            %space.  Then the output is five points each with three components.
            %Each point is a column in a matrix with three rows.  The first column is
            %L1, the second L2, and so on to the last which is L5.

            %Compute the location of the libration points
            l=1-mu;

            LP = zeros(5,3);

            %L1
            p_L1=[1, 2*(mu-l), l^2-4*l*mu+mu^2, 2*mu*l*(l-mu)+mu-l, mu^2*l^2+2*(l^2+mu^2), mu^3-l^3];
            L1roots=roots(p_L1);
            %initialize L1 for loop
            L1=0;
            for i=1:5
                if (L1roots(i) > -mu) && (L1roots(i) < l)
                    L1=L1roots(i);
                end
            end
            LP(1,1) = L1;

            %L2
            p_L2=[1, 2*(mu-l), l^2-4*l*mu+mu^2, 2*mu*l*(l-mu)-(mu+l), mu^2*l^2+2*(l^2-mu^2), -(mu^3+l^3)];
            L2roots=roots(p_L2);
            %initialize L2 for loop
            L2=0;
            for i=1:5
                if (L2roots(i) > -mu) && (L2roots(i) > l)
                    L2=L2roots(i);
                end
            end
            LP(2,1) = L2;

            %L3
            p_L3=[1, 2*(mu-l), l^2-4*mu*l+mu^2, 2*mu*l*(l-mu)+(l+mu), mu^2*l^2+2*(mu^2-l^2), l^3+mu^3];
            L3roots=roots(p_L3);
            %initialize L3 for loop
            L3=0;
            for i=1:5
                if L3roots(i) < -mu
                    L3=L3roots(i);
                end
            end
            LP(3,1) = L3;

            %L4
            LP(4,1) = 0.5 - mu;
            LP(4,2) = sqrt(3)/2;

            %L5
            LP(5,1) = 0.5 - mu;
            LP(5,2) = -sqrt(3)/2;
        end

        function xdot = stateTransMatrixODE(t,xinput,mu)
            xstate = xinput(1:6);
            xstatedot = HaloOrbitApproximator.cr3bpODE(t,xstate,mu);

            PHI = reshape(xinput(7:6*6+6),6,6);

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

            x1 = -mu;
            x2 = 1-mu;

            r1 = sqrt((x-x1)^2 + y^2 + z^2);
            r2 = sqrt((x-x2)^2 + y^2 + z^2);

            OMEGA_XX = 1 + (1-mu)*((3*(x-x1)^2/r1^5) - 1/r1^3) ...
                + mu*((3*(x-x2)^2/r2^5) - 1/r2^3);

            OMEGA_YY = 1 + (1-mu)*((3*y^2/r1^5) - 1/r1^3) ...
                + mu*((3*y^2/r2^5) - 1/r2^3);

            OMEGA_XY = 3*(1-mu)*(x-x1)*y/r1^5 + 3*mu*(x-x2)*y/r2^5;

            OMEGA_XZ = 3*(1-mu)*z*(x-x1)/r1^5 + 3*mu*z*(x-x2)/r2^5;

            OMEGA_YZ = 3*(1-mu)*y*z/r1^5 + 3*mu*y*z/r2^5;

            OMEGA_ZZ = 3*(1-mu)*z^2/r1^5 - (1-mu)/r1^3 + 3*mu*z^2/r2^5 - mu/r2^3;

            A  = [0 0 0 1 0 0;
                0 0 0 0 1 0;
                0 0 0 0 0 1;
                OMEGA_XX OMEGA_XY OMEGA_XZ 0 2 0;
                OMEGA_XY OMEGA_YY OMEGA_YZ -2 0 0;
                OMEGA_XZ OMEGA_YZ OMEGA_ZZ 0 0 0];
            PHIdot = A * PHI;

            xdot = [xstatedot;reshape(PHIdot,6*6,1)];
        end

        function ydot = cr3bpODE(t,y, mu)
            %Define the distances from 1 and 2 to the s/c
            r1 = sqrt((y(1)+mu)^2 + y(2)^2 + y(3)^2);
            r2 = sqrt((y(1)-(1-mu))^2 + y(2)^2 + y(3)^2);

            %the deriative functions here
            %velocities
            ydot(1) = y(4);
            ydot(2) = y(5);
            ydot(3) = y(6);

            %accelerations
            ydot(4) = 2*y(5) + y(1) - ((1-mu)*(y(1)+mu))/r1^3 - (mu*(y(1)-1+mu))/r2^3;
            ydot(5) = -2*y(4) + y(2) - ((1-mu)*y(2))/r1^3 - mu*y(2)/r2^3;
            ydot(6) = (-(1-mu)*y(3))/r1^3 - (mu*y(3))/r2^3;

            ydot=ydot';
        end
    end
end

%         function [k, lambda, omega, ...
%                   a21, a22, a23, a24, a31, a32, ...
%                   b21, b22, b31, b32, ...
%                   d21, d31, d32, Ax, Ay, Az] = getRichardsonConstants(obj)
%
%             c2 = (1/obj.gL^3) * (obj.mu + ((1-obj.mu)*obj.gL^3)/(1-obj.gL)^3);
%             c3 = (1/obj.gL^3) * (obj.mu - ((1-obj.mu)*obj.gL^4)/(1-obj.gL)^4);
%             c4 = (1/obj.gL^3) * (obj.mu + ((1-obj.mu)*obj.gL^5)/(1-obj.gL)^5);
%
%             p = [1, 0, (c2-2), 0, -(c2-1)*(1+2*c2)];
%             p_roots = roots(p);
%             lambda = real(p_roots(real(p_roots) > 0 & imag(p_roots) == 0));
%
%             k = 2*lambda/(lambda^2 + 1 - c2);
%
%             d1 = ((3*lambda^2)/k) * (k*(6*lambda^2-1)-2*lambda);
%             d2 = ((8*lambda^2)/k) * (k*(11*lambda^2-1)-2*lambda);
%
%             a21 = (3*c3*(k^2-2))/(4*(1+2*c2));
%             a22 = (3*c3)/(4*(1+2*c2));
%             a23 = ((-3*c3*lambda)/(4*k*d1))*(3*(k^3)*lambda - 6*k*(k-lambda) + 4);
%             a24 = ((-3*c3*lambda)/(4*k*d1))*(2+3*k*lambda);
%
%             d21 = -c3/(2*lambda^2);
%             d31 = (3/(64*lambda^2)) * (4*c3*a24 + c4);
%             d32 = (3/(64*lambda^2)) * (4*c3*(a23 - d21) + c4*(4+k^2));
%
%             b21 = ((-3*c3*lambda)/(2*d1))*(3*k*lambda - 4);
%             b22 = (3*c3*lambda)/d1;
%             b31 = (3/(8*d2)) * (8*lambda*(3*c3*(k*b21 - 2*a23) - c4*(2+3*k^2)) + ...
%                                          (9*lambda^2 + 1 + 2*c2)*(4*c3*(k*a23-b21)+(k*c4*(4+k^2))));
%             b32 = (1/d2) * (9*lambda*(c3*(k*b22 + d21 - 2*a24) - c4) + ...
%                                          (3/8)*(9*lambda^2 + 1 + 2*c2)*(4*c3*(k*a24-b22)+(k*c4)));
%
%             a31 = (-9*lambda/(4*d2)) * (4*c3*(k*a23 - b21) + k*c4*(4+k^2)) + ...
%                                          ((9*lambda^2 + 1 - c2)/(2*d2))*(3*c3*(2*a23-k*b21)+(c4*(2+3*k^2)));
%             a32 = (-1/d2) * ((9/4)*lambda*(4*c3*(k*a24 - b22) + k*c4) + ...
%                                           (3/2)*(9*lambda^2 + 1 - c2)*(c3*(k*b22 + d21 - 2*a24) - c4));
%
%             s1 = (1/(2*lambda*(lambda*(1+k^2)-2*k))) * ((3/2)*c3*(2*a21*(k^2-2) - a23*(k^2+2) - 2*k*b21) - ...
%                                                         (3/8)*c4*(3*k^4 - 8*k^2 + 8));
%             s2 = (1/(2*lambda*(lambda*(1+k^2)-2*k))) * ((3/2)*c3*(2*a22*(k^2-2) + a24*(k^2+2) + 2*k*b22 + 5*d21) + ...
%                                                         (3/8)*c4*(12 - k^2));
%
%             a1 = (-3/2)*c3*(2*a21 + a23 + 5*d21) - (3/8)*c4*(12-k^2);
%             a2 = (3/2)*c3*(a24 - 2*a22) + (9/8)*c4;
%
%             l1 = a1 + 2*(lambda^2)*s1;
%             l2 = a2 + 2*(lambda^2)*s2;
%
%             delta = lambda^2 - c2;
%
%             Az = (obj.AzUnscaled/obj.LU)*obj.gL;
%             Ay = Az/obj.gL;
%             Ax = sqrt((-l2 * Az^2 - delta)/l1);
%
%             omega = 1 + s1*Ax^2 + s2*Az^2;
%         end


%this is an example, demonstrating a Radial Basis Function method of vector
%acoustic tomography, using acoustic time-of-flight tomography as an
%example.
%This involves sending a sonic signal across a measurement area and timing
%how long it takes to travel.  A number of transducers are situated around
%the measurement area to achieve this.  Since the absolute sonic speed is
%effected by temperature and wind speed, so is the time of flight.  It is
%possible to reconstruct the temperature and wind velocity from the
%collection of time-of-flight data.
%For more information, please see ref [1] or http://blog.nutaksas.com
%
%References
%Wiens, Travis "Sensing of Turbulent Flows Using Real-Time Acoustic
%Tomography." X1Xth Biennial Conference of the NewZealand Acoustical
%Society, 2008.

%    Copyright Travis Wiens 2008
%
%    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/>.
%
%    If you would like to request that this software be licensed under a less
%    restrictive license (i.e. for commercial closed-source use) please
%    contact Travis at travis.mlfx@nutaksas.com

N_xd=16;%number of transducers
sig_dt=1e-6;%(s) std of error to add to dt
N_r=25;%number of radial basis functions

recalculate=true;%set this to false to skip recalculating
rbf_data_fname='gerris_rbf_data.mat';
if recalculate

    fprintf('Calculating forward problem...')
    gerris_fname='simsave-5.0.gfs';
    %this data was calculated using the Gerris Flow Solver
    %(http:gfs.sourceforge.net), from the Karman vortex street example

    %since Gerris data is not regular, we'll fit a RBF to the temperature
    %and velocity data, then use that to calculate the flight times
    data=load(gerris_fname);%load [x y z u v]

    N_rbf_f=300;%number of rbf centers for forward problem
    X_scale=10;%rescale data
    Sc_G=[3 3]*X_scale;%scale factor for gerris
    OS_G=[1 0];%(m) offset for gerris data

    X_limits=[-2 2;-2 2]*X_scale;%limits for training
    Xc_fu=[X_limits(1,1)+diff(X_limits(1,:))*rand(1,N_rbf_f);X_limits(2,1)+diff(X_limits(2,:))*rand(1,N_rbf_f)]';%model rbf centres
    Xc_fv=[X_limits(1,1)+diff(X_limits(1,:))*rand(1,N_rbf_f);X_limits(2,1)+diff(X_limits(2,:))*rand(1,N_rbf_f)]';
    Xc_fT=[X_limits(1,1)+diff(X_limits(1,:))*rand(1,N_rbf_f);X_limits(2,1)+diff(X_limits(2,:))*rand(1,N_rbf_f)]';

    x_G=(data(:,1)-OS_G(1))*Sc_G(1);%x for Gerris data
    y_G=(data(:,2)-OS_G(2))*Sc_G(2);%x for Gerris data
    U_idx=find((x_G>X_limits(1,1))&(x_G<X_limits(1,2))&(y_G>X_limits(2,1))&(y_G<X_limits(2,2)));
    x_G=x_G(U_idx);
    y_G=y_G(U_idx);

    k_fu=1*ones(N_rbf_f,1)/X_scale^2;%prescalar
    k_fv=1*ones(N_rbf_f,1)/X_scale^2;
    k_fT=4*ones(N_rbf_f,1)/X_scale^2;
    k_fu(1)=0;%allow for bias
    k_fv(1)=0;
    k_fT(1)=0;

    u0=mean(data(U_idx,4));%(m/s) base velocity
    v0=mean(data(U_idx,5));%(m/s) base velocity
    T=293+data(U_idx,6);%(K)  temperature
    T0=mean(T);%(K) base temperature
    [W_fu]=train_rbf([x_G y_G],data(U_idx,4)-u0,Xc_fu,k_fu);
    [W_fv]=train_rbf([x_G y_G],data(U_idx,5)-v0,Xc_fv,k_fv);
    [W_fT]=train_rbf([x_G y_G],T-T0,Xc_fT,k_fT);

    %set up transducers in a square
    N_dim=2;%number of dimensions
    X_xd=zeros(N_xd,N_dim);%XD positions
    phi=linspace(0,2*pi,N_xd+1);%angle of XD
    phi=phi+0.01*randn(size(phi));%move the xducers a bit
    r_xd=1*X_scale;%radius of circle inscribed in square
    X_xd_c=mean(X_limits,2)';%centre of circle
    for i=1:N_xd;
        if abs(tan(phi(i)))<1%intersects with verical lines
            X_xd(i,:)=X_xd_c+r_xd*[sign(cos(phi(i))) tan(phi(i))*sign(cos(phi(i)))];%set XD on a square
        else
            X_xd(i,:)=X_xd_c+r_xd*[ sign(sin(phi(i)))*tan(phi(i)).^-1 sign(sin(phi(i)))];%set XD on a square
        end
    end

    %now calculate pairs of XD to make paths
    X_xd0=[];
    X_xd1=[];
    idx=[];
    count=1;
    for i=1:N_xd
        for j=1:N_xd
            if i~=j%ignore path from XD to itself
                X_xd0(count,:)=X_xd(i,:);
                X_xd1(count,:)=X_xd(j,:);
                idx(count,:)=[i j];
                count=count+1;
            end
        end
    end
    N_paths=count-1;%number of paths

    X_limits_plot=(repmat(X_xd_c,2,1)+[-r_xd -r_xd;+r_xd +r_xd])';%limits to plot
    N_plot1=[25 25];%number of points to plot [x y]
    x_plot1=linspace(X_limits_plot(1,1),X_limits_plot(1,2),N_plot1(1))';
    y_plot1=linspace(X_limits_plot(2,1),X_limits_plot(2,2),N_plot1(2))';
    [x_mesh1 y_mesh1]=meshgrid(x_plot1,y_plot1);
    X_plot1=[reshape(x_mesh1,[],1) reshape(y_mesh1,[],1)];

    N_plot2=[100 100];%%higher resolution
    x_plot2=linspace(X_limits_plot(1,1),X_limits_plot(1,2),N_plot2(1))';
    y_plot2=linspace(X_limits_plot(2,1),X_limits_plot(2,2),N_plot2(2))';
    [x_mesh2 y_mesh2]=meshgrid(x_plot2,y_plot2);
    X_plot2=[reshape(x_mesh2,[],1) reshape(y_mesh2,[],1)];

    [u_rbf]=sim_rbf(Xc_fu,X_plot1,W_fu,k_fu);%simulate rbf fields
    [v_rbf]=sim_rbf(Xc_fv,X_plot1,W_fv,k_fv);
    [T_rbf]=sim_rbf(Xc_fT,X_plot1,W_fT,k_fT);

    [u_rbf2]=sim_rbf(Xc_fu,X_plot2,W_fu,k_fu);
    [v_rbf2]=sim_rbf(Xc_fv,X_plot2,W_fv,k_fv);
    [T_rbf2]=sim_rbf(Xc_fT,X_plot2,W_fT,k_fT);

    %calculate flight times analytically from RBF networks
    c0=340;%(m/s) sonic speed

    u_int=rbfn_integral(Xc_fu,X_xd0,X_xd1,W_fu,k_fu);%line integrals across rbfn
    v_int=rbfn_integral(Xc_fv,X_xd0,X_xd1,W_fv,k_fv);
    T_int=rbfn_integral(Xc_fT,X_xd0,X_xd1,W_fT,k_fT);
    theta=atan2(X_xd1(:,2)-X_xd0(:,2),X_xd1(:,1)-X_xd0(:,1));%(rad) angle of lines
    l=sqrt(sum((X_xd1-X_xd0).^2,2));%(m) path length
    dt0=l./c0.*(1-(u0*cos(theta)+v0*sin(theta))/c0)-...
            1/c0*(T_int/(2*T0)+u_int.*cos(theta)/c0+v_int.*sin(theta)/c0);%(s) time of flight

    save(rbf_data_fname,'dt0','l','theta','c0','T0','u_rbf','v_rbf','T_rbf','u_rbf2','v_rbf2','T_rbf2','X_plot1',...
        'X_plot2','x_plot1','x_plot2','y_plot1','y_plot2','N_plot1','N_plot2','N_paths','N_dim','X_xd','X_xd0',...
        'X_xd1','r_xd','u0','v0','X_xd_c','X_limits','X_scale','X_limits_plot')
    fprintf('done\n')
else
    load(rbf_data_fname)
end

%%%%%%%%%%%%%
%%%%%%%%%%%
%inversion
%%%%%%%%%%
%%%%%%%%%%%%

fprintf('Initializing inversion...')

basisfunction='gaussian';
k_scale=1/X_scale^2;%prescaler for basis function
alpha_scale=1.0e-6;%parameter for regularization alpha

dt=dt0+sig_dt*randn(size(dt0));%add noise to time of flight measurements

Xc_i=[X_limits(1,1)+diff(X_limits(1,:))*rand(1,N_r);X_limits(2,1)+diff(X_limits(2,:))*rand(1,N_r)]';%inversion rbf centres
k_i=k_scale*ones(N_r,1);
Omegapsi=Omegapsi_rbf(Xc_i,X_xd0,X_xd1,k_i,c0,basisfunction);
OmegaT=rbf_integral(Xc_i,X_xd0,X_xd1,k_i,basisfunction )/(c0*2*T0);
Omega=[OmegaT Omegapsi];
alpha_reg=alpha_scale*norm(Omega'*Omega);%regulatization parameter
A_rbf=pinv(Omega'*Omega+alpha_reg*eye(size(Omega,2)))*Omega';%solve!

%calculate matrices to get fields from weights
[zeta_u1 zeta_v1]=zeta_uv(Xc_i,X_plot1,k_i,basisfunction);
[zeta_u2 zeta_v2]=zeta_uv(Xc_i,X_plot2,k_i,basisfunction);
W_T_tmp=zeros(N_r,1);%placeholder
[tmp zeta_T1]=sim_rbf(Xc_i,X_plot1,W_T_tmp,k_i,basisfunction);
[tmp zeta_T2]=sim_rbf(Xc_i,X_plot2,W_T_tmp,k_i,basisfunction);

fprintf('done\n')

fprintf('Solving for weights...')
d_rbf=-dt+l/c0.*(1-(u0*cos(theta)+v0*sin(theta))/c0);%data vector
W=A_rbf*d_rbf;%weights
W_T=W(1:N_r);
W_U=W((N_r+1):end);
fprintf('done\n')

fprintf('Calculating fields...')
%calculate fields
u_hat1=zeta_u1*W_U;
v_hat1=zeta_v1*W_U;
u_hat2=zeta_u2*W_U;
v_hat2=zeta_v2*W_U;
T_hat1=zeta_T1*W_T;
T_hat2=zeta_T2*W_T;
fprintf('done\n')

EU_rms=sqrt(mean((u_rbf2-u_hat2).^2+(v_rbf2-v_hat2).^2));
ET_rms=sqrt(mean((T_rbf2-T_hat2).^2));

fprintf('EU_rms=%f m/s ET_rms=%f K\n',EU_rms,ET_rms)
fprintf('Cond(Omega^T*Omega)=%0.2e, Cond(Omega^T*Omega+alpha*eye)=%0.2e\n',cond(Omega'*Omega),cond(Omega'*Omega+alpha_reg*eye(size(Omega,2))))
fprintf('%d data, %d parameters, ratio=%f\n',size(Omega,1),size(Omega,2),size(Omega,2)/size(Omega,1))

figure(1)
clim=[min([T_rbf2; T_hat2]) max([T_rbf2; T_hat2])];
clf
subplot(2,1,1)
plot(nan,nan)
hold on
imagesc(x_plot2,y_plot2,reshape(T_rbf2,N_plot2(1),[]),clim)
plot([X_xd0(:,1) X_xd1(:,1)]',[X_xd0(:,2) X_xd1(:,2)]','wx:')
hold off
h=colorbar;
ylabel(h,'dT (K)')
axis tight
xlabel('x (m)')
ylabel('y (m)')
title('Target')

subplot(2,1,2)
plot(nan,nan)
hold on
imagesc(x_plot2,y_plot2,reshape(T_hat2,N_plot2(1),[]),clim)
plot([X_xd0(:,1) X_xd1(:,1)]',[X_xd0(:,2) X_xd1(:,2)]','wx:')
hold off
h=colorbar;
ylabel(h,'dT_{hat} (K)')
axis tight
xlabel('x (m)')
ylabel('y (m)')
title('Reconstruction')

figure(2)
plot(nan,nan)
hold on
imagesc(x_plot2,y_plot2,reshape(sqrt((T_hat2-T_rbf2).^2),N_plot2(1),[]))
%plot([X_xd0(:,1) X_xd1(:,1)]',[X_xd0(:,2) X_xd1(:,2)]','kx-')
hold off
h=colorbar;
ylabel(h,'RMSE (K)')
axis tight
xlabel('x (m)')
ylabel('y (m)')

figure(3)
plot(nan,nan)
hold on
imagesc(x_plot2,y_plot2,reshape(sqrt((u_hat2-u_rbf2).^2+(v_hat2-v_rbf2).^2),N_plot2(1),[]))
h=colorbar;
h1=quiver(x_plot1,y_plot1,reshape(u_rbf,N_plot1(1),[]),reshape(v_rbf,N_plot1(1),[]),'k');
h2=quiver(x_plot1,y_plot1,reshape(u_hat1,N_plot1(1),[]),reshape(v_hat1,N_plot1(1),[]));
hold off
axis tight
xlabel('x (m)')
ylabel('y (m)')
ylabel(h,'E (m/s)')
legend([h1;h2],'Target','Reconstruction')


function [t,x_new,f_new,g_new,funEvals,H] = ArmijoBacktrack(...
    x,t,d,f,fr,g,gtd,c1,LS,tolX,debug,doPlot,saveHessianComp,funObj,varargin)
%
% Backtracking linesearch to satisfy Armijo condition
%
% Inputs:
%   x: starting location
%   t: initial step size
%   d: descent direction
%   f: function value at starting location
%   fr: reference function value (usually funObj(x))
%   gtd: directional derivative at starting location
%   c1: sufficient decrease parameter
%   debug: display debugging information
%   LS: type of interpolation
%   tolX: minimum allowable step length
%   doPlot: do a graphical display of interpolation
%   funObj: objective function
%   varargin: parameters of objective function
%
% Outputs:
%   t: step length
%   f_new: function value at x+t*d
%   g_new: gradient value at x+t*d
%   funEvals: number function evaluations performed by line search
%   H: Hessian at initial guess (only computed if requested

% Evaluate the Objective and Gradient at the Initial Step
if nargout == 6
    [f_new,g_new,H] = feval(funObj, x + t*d, varargin{:});
else
    [f_new,g_new] = feval(funObj, x + t*d, varargin{:});
end
funEvals = 1;

while f_new > fr + c1*t*gtd || ~isLegal(f_new)

    temp = t;
    if LS == 0 || ~isLegal(f_new)
        % Backtrack w/ fixed backtracking rate
        if debug
            fprintf('Fixed BT\n');
        end
        t = 0.5*t;
    elseif LS == 2 && isLegal(g_new)
        % Backtracking w/ cubic interpolation w/ derivative
        if debug
            fprintf('Grad-Cubic BT\n');
        end
        t = polyinterp([0 f gtd; t f_new g_new'*d],doPlot);
    elseif funEvals < 2 || ~isLegal(f_prev)
        % Backtracking w/ quadratic interpolation (no derivative at new point)
        if debug
            fprintf('Quad BT\n');
        end
        t = polyinterp([0 f gtd; t f_new sqrt(-1)],doPlot);
    else%if LS == 1
        % Backtracking w/ cubic interpolation (no derivatives at new points)
        if debug
            fprintf('Cubic BT\n');
        end
        t = polyinterp([0 f gtd; t f_new sqrt(-1); t_prev f_prev sqrt(-1)],doPlot);
    end

    % Adjust if change in t is too small/large

    if t < temp*1e-3
        if debug
            fprintf('Interpolated Value Too Small, Adjusting\n');
        end
        t = temp*1e-3;
    elseif t > temp*0.6
        if debug
            fprintf('Interpolated Value Too Large, Adjusting\n');
        end
        t = temp*0.6;
    end

    f_prev = f_new;
    t_prev = temp;
    if ~saveHessianComp && nargout == 6
        [f_new,g_new,H] = feval(funObj, x + t*d, varargin{:});
    else
        [f_new,g_new] = feval(funObj, x + t*d, varargin{:});
    end
    funEvals = funEvals+1;

    % Check whether step size has become too small
    if sum(abs(t*d)) <= tolX
        if debug
            fprintf('Backtracking Line Search Failed\n');
        end
        t = 0;
        f_new = f;
        g_new = g;
        break;
    end
end

% Evaluate Hessian at new point
if nargout == 6 && funEvals > 1 && saveHessianComp
    [f_new,g_new,H] = feval(funObj, x + t*d, varargin{:});
    funEvals = funEvals+1;
end

x_new = x + t*d;

end