function varargout = warpvars_vbmc(varargin)
%WARPVARS Linear and nonlinear transformation of variables.
%
%  TRINFO = PDFTRANS(NVARS,LB,UB) returns the transformation structure
%  TRINFO for a problem with NVARS dimensions and lower/upper bounds
%  respectively LB and UB. LB and UB are either scalars or row arrays that
%  can contain real numbers and Inf's
%  The ordering LB <= UB needs to hold coordinate-wise.
%
%  Variables with lower or upper bounds are transformed via a log transform.
%  Variables with both lower and upper bounds are transformed via a logit
%  transform.
%
%  Y = TRANSVARS(X,'dir',TRINFO) performs direct transform of constrained
%  variables X into unconstrained variables Y according to transformation
%  encoded in structure TRINFO. X must be a N x NVARS array, where N is the
%  number of input data and NVARS is the number of dimensions.
%
%  X = TRANSVARS(Y,'inv',TRINFO) performs inverse transform of unconstrained
%  variables Y into constrained variables X.
%
%  P = TRANSVARS(Y,'prob',TRINFO) returns probability multiplier for the
%  original pdf evaluated at f^{-1}(Y), that is | df^{-1}(y) / dy |.
%
%  LP = TRANSVARS(Y,'logprob',TRINFO) returns log probability term for the
%  original log pdf evaluated at f^{-1}(Y).

%  Author: Luigi Acerbi
%  e-mail: luigi.acerbi@gmail.com

if nargin < 3
    error('TRANSVARS requires a minimum of three input arguments.');
end

%% Transform variables
if nargin == 3 && (isstruct(varargin{3}) || ischar(varargin{2}))

    Tol = sqrt(eps);    % Small number

    action = varargin{2};
    trinfo = varargin{3};

    if isempty(action)
        error('The transformation direction cannot be empty. Allowed values are direct (''dir'' or ''d'') and inverse (''inv'' or ''i'').');
    end

    if isempty(trinfo)
        % Empty TRINFO - consider as identity transformation

        x = varargin{1};

        switch lower(action(1))
            case {'d','i'}
                varargout{1} = x;
            case 'p'
                varargout{1} = ones(size(x,1),1);
            case 'l'
                varargout{1} = zeros(size(x,1),1);
            case {'m','f','g'}
                error('TRINFO is empty.');
            otherwise
                error(['Unkwnown transformation direction ''' action '''. Allowed values are direct (''dir'' or ''d'') and inverse (''inv'' or ''i'').']);
        end
    else

        scale = [];
        if isfield(trinfo,'scale') && ~isempty(trinfo.scale) && any(trinfo.scale ~= 1)
            scale = trinfo.scale;
        end

        if ~isfield(trinfo,'R_mat'); trinfo.R_mat = []; end

        nvars = numel(trinfo.lb_orig);  % Number of variables

        switch lower(action(1))
        %% DIRECT TRANSFORM
            case 'd'    % Direct transform
                x = varargin{1};
                y = x;
                a = trinfo.lb_orig;
                b = trinfo.ub_orig;
                mu = trinfo.mu;
                delta = trinfo.delta;

                % Unbounded scalars (possibly center and rescale)
                idx = trinfo.type == 0;
                if any(idx)
                    y(:,idx) = bsxfun(@rdivide,bsxfun(@minus,x(:,idx),mu(idx)),delta(idx));
                end

                % Lower bounded scalars
                idx = trinfo.type == 1;
                if any(idx)
                    y(:,idx) = log(bsxfun(@minus, x(:,idx), a(idx)));
                end

                % Upper bounded scalars
                idx = trinfo.type == 2;
                if any(idx)
                    y(:,idx) = log(bsxfun(@minus, b(idx), x(:,idx)));
                end

                % Lower and upper bounded scalars
                idx = trinfo.type == 3;
                if any(idx)
                    z = bsxfun(@rdivide, bsxfun(@minus, x(:,idx), a(idx)), ...
                        b(idx) - a(idx));
                    y(:,idx) = log(z./(1-z));
                    y(:,idx) = bsxfun(@rdivide,bsxfun(@minus,y(:,idx),mu(idx)),delta(idx));
                end

                % Lower and upper bounded scalars with Beta CDF transform
                idx = trinfo.type == 4;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        % z = betacdf((x(:,ii) - a(ii)) / (b(ii) - a(ii)),alpha(ii),beta(ii));
                        z = min(max(eps,betacdf((x(:,ii) - a(ii)) / (b(ii) - a(ii)),alpha(ii),beta(ii))),1-eps);
                        y(:,ii) = log(z./(1-z));
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy CDF transform
                idx = trinfo.type == 5;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        % z = kumarcdf((x(:,ii) - a(ii)) / (b(ii) - a(ii)),alpha(ii),beta(ii));
                        % p = min(max(eps,kumarcdf(z,alpha(ii),beta(ii))),1-eps);
                        % y(:,ii) = log(p./(1-p));
                        z = (x(:,ii) - a(ii)) / (b(ii) - a(ii));
                        % p = 1 - (1 - z.^alpha(ii)).^beta(ii);
                        y(:,ii) = log1p(-(1 - z.^alpha(ii)).^beta(ii)) - beta(ii)*log1p(-z.^alpha(ii));
                    end
                     y(:,idx) = bsxfun(@rdivide,bsxfun(@minus,y(:,idx),mu(idx)),delta(idx));
                end

                % Lower and upper bounded scalars with Kumaraswamy-logistic-power transform
                idx = trinfo.type == 6;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    mu = trinfo.mu;
                    gamma = trinfo.gamma;
                    for ii = find(idx)
                        z = (x(:,ii) - a(ii)) / (b(ii) - a(ii));
                        y(:,ii) = log1p(-(1 - z.^alpha(ii)).^beta(ii)) - beta(ii)*log1p(-z.^alpha(ii)) - mu(ii);
                        y(:,ii) = sign(y(:,ii)).*abs(y(:,ii)).^gamma(ii);
                    end
                end

                % Lower and upper bounded scalars with nonparametric CDF transform
                idx = trinfo.type == 7;
                if any(idx)
                    xspace = trinfo.xspace;
                    pspace = trinfo.pspace;
                    for ii = find(idx)
                        y(:,ii) = norminv(interp1(xspace(ii,:),pspace(ii,:),x(:,ii)));
                    end
                end

                % Lower and upper bounded scalars with GMM CDF transform
                idx = trinfo.type == 8;
                if any(idx)
                    for ii = find(idx)
                        gmm = trinfo.gmm{ii};
                        z = (x(:,ii) - a(ii)) / (b(ii) - a(ii));
                        z = gmm.lambda*z + (1-gmm.lambda)*(gmm1cdf(z,gmm.w,gmm.Mu,gmm.Sigma)-gmm.Min)./gmm.Norm;
                        y(:,ii) = norminv(z);
                        % y(:,ii) = logiinv(z);
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logit transform
                idx = trinfo.type == 9;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        z = (x(:,ii) - a(ii)) / (b(ii) - a(ii));
                        u = z.^(alpha(ii));

                        % Small u (close to zero)
                        uzero = u < sqrt(eps);
                        y(uzero,ii) = log(beta(ii)) + alpha(ii)*log(z(uzero)) + 0.5*u(uzero)*(beta(ii)+1) + 0.5*((beta(ii)-1)^2/4-beta(ii))*u(uzero).^2;

                        % Large u (close to 1)
                        uone = u > (1 - sqrt(eps));
                        y(uone,ii) = log(1./(1 - u(uone)).^beta(ii) - 1);

                        % Other values
                        y(~uzero & ~uone,ii) = log1p(-(1 - z(~uzero & ~uone).^alpha(ii)).^beta(ii)) - beta(ii)*log1p(-z(~uzero & ~uone).^alpha(ii));
                    end
                end

                % Unbounded with logistic-Kumaraswamy-logit transform
                idx = trinfo.type == 10;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        z = (x(:,ii)-mu(:,ii)) ./ delta(:,ii);
                        %z = exp(z)./(exp(z)+1);
                        %y(:,ii) = log1p(-(1 - z.^alpha(:,ii)).^beta(:,ii)) - beta(:,ii).*log1p(-z.^alpha(:,ii));
                        s = 1./(1+exp(-z));
                        u = s.^alpha(ii);

                        % Small u (near zero)
                        uzero = u < Tol;
                        %y(uzero,ii) = log(beta(ii)) + alpha(ii)*log(s(uzero)) + beta(ii)*u(uzero);
                        y(uzero,ii) = log(beta(ii)) + alpha(ii)*z(uzero) + beta(ii)*u(uzero);

                        % Large u (near one)
                        uone = u > (1 - Tol);
                        w = 1./(1 + exp(z(uone)));
                        y(uone,ii) = -log(alpha(ii)) + beta(ii)*z(uone) - (alpha(ii)*w).^beta(ii);

                        % All other cases
                        urest = ~uzero & ~uone;
                        y(urest,ii) = log1p(-(1 - s(urest).^alpha(ii)).^beta(ii)) - beta(ii).*log1p(-s(urest).^alpha(ii));
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logit inverse CDF transform
                idx = trinfo.type == 11;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        % z = kumarinv((x(:,ii) - a(ii)) / (b(ii) - a(ii)),alpha(ii),beta(ii));
                        % p = min(max(eps,kumarcdf(z,alpha(ii),beta(ii))),1-eps);
                        % y(:,ii) = log(p./(1-p));
                        z = (x(:,ii) - a(ii)) / (b(ii) - a(ii));
                        % p = 1 - (1 - z.^alpha(ii)).^beta(ii);
                        % y(:,ii) = 1./alpha(ii).*log1p(-(1 - z).^(1./beta(ii))) - log1p(-(1-(1-z).^(1./beta(ii))).^(1./alpha(ii)));
                        u = (1./beta(ii)).*log1p(-z);

                        uidx = u < log(eps)/2;
                        % u(uidx) = 1 - 1./alpha(ii).*exp(u(uidx));
                        y(uidx,ii) = -1./alpha(ii).*exp(u(uidx)) + log(alpha(ii)) - u(uidx);
                        % u(~uidx) = (1 - exp(u(~uidx))).^(1/alpha(ii));
                        y(~uidx,ii) = (1/alpha(ii)).*log1p(-exp(u(~uidx))) - log1p(-(1 - exp(u(~uidx))).^(1/alpha(ii)));
                        usmall = abs(u) < sqrt(eps);
                        y(usmall,ii) = (1/alpha(ii)).*log(-u(usmall)) - log1p(-u(usmall)/alpha(ii));

                        % y(:,ii) = log(expm1(-1./alpha(ii).*log1p(-(1-z).^(1./beta(ii)))));
                        y(:,ii) = bsxfun(@rdivide,bsxfun(@minus,y(:,ii),mu(ii)),delta(ii));
                    end
                end

                % Lower and upper bounded scalars (cumulative normal)
                idx = trinfo.type == 12;
                if any(idx)
                    z = bsxfun(@rdivide, bsxfun(@minus, x(:,idx), a(idx)), ...
                        b(idx) - a(idx));
                    y(:,idx) = -sqrt(2).*erfcinv(2*z);
                    y(:,idx) = bsxfun(@rdivide,bsxfun(@minus,y(:,idx),mu(idx)),delta(idx));
                end

                % Lower and upper bounded scalars (Student-t, nu = 4)
                idx = trinfo.type == 13;
                if any(idx)
                    z = bsxfun(@rdivide, bsxfun(@minus, x(:,idx), a(idx)), ...
                        b(idx) - a(idx));
                    aa = sqrt(4*z.*(1-z));
                    q = cos(acos(aa)/3)./aa;
                    y(:,idx) = sign(z - 0.5).*(2.*sqrt(q-1));
                    y(:,idx) = bsxfun(@rdivide,bsxfun(@minus,y(:,idx),mu(idx)),delta(idx));
                end

                % Rotate output
                if ~isempty(trinfo.R_mat); y = y*trinfo.R_mat; end

                % Rescale output
                if ~isempty(scale); y = bsxfun(@rdivide,y,scale); end

                varargout{1} = y;

            %% INVERSE TRANSFORM
            case 'i'    % Inverse transform
                y = varargin{1};
                % Rescale input
                if ~isempty(scale); y = bsxfun(@times,y,scale); end

                % Rotate input
                if ~isempty(trinfo.R_mat); y = y*trinfo.R_mat'; end

                x = y;
                a = trinfo.lb_orig;
                b = trinfo.ub_orig;
                mu = trinfo.mu;
                delta = trinfo.delta;

                % Unbounded scalars (possibly unscale and uncenter)
                idx = trinfo.type == 0;
                if any(idx)
                    x(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                end

                % Lower bounded scalars
                idx = trinfo.type == 1;
                if any(idx)
                    x(:,idx) = bsxfun(@plus, exp(y(:,idx)), a(idx));
                end

                % Upper bounded scalars
                idx = trinfo.type == 2;
                if any(idx)
                    x(:,idx) = bsxfun(@minus, b(idx), exp(y(:,idx)));
                end

                % Lower and upper bounded scalars
                idx = trinfo.type == 3;
                if any(idx)
                    x(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    x(:,idx) = bsxfun(@plus, a(:,idx), bsxfun(@times, ...
                        b(idx)-a(idx), 1./(1+exp(-x(:,idx)))));
                end

                % Lower and upper bounded scalars with Beta CDF transform
                idx = trinfo.type == 4;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        z = 1./(1+exp(-y(:,ii)));
                        x(:,ii) = a(ii) + (b(ii)-a(ii))*betainv(z,alpha(ii),beta(ii));
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy CDF transform
                idx = trinfo.type == 5;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    y(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    for ii = find(idx)
                        % z = 1./(1+exp(-y(:,ii)));
                        % x(:,ii) = a(ii) + (b(ii)-a(ii))*kumarinv(z,alpha(ii),beta(ii));
                        z = exp(-y(:,ii))./(1+exp(-y(:,ii)));
                        x(:,ii) = a(ii) + (b(ii)-a(ii))*(1-z.^(1/beta(ii))).^(1/alpha(ii));
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logistic-power transform
                idx = trinfo.type == 6;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    mu = trinfo.mu;
                    gamma = trinfo.gamma;
                    for ii = find(idx)
                        z = sign(y(:,ii)).*abs(y(:,ii)).^(1/gamma(ii)) + mu(ii);
                        z = exp(-z)./(1+exp(-z));
                        x(:,ii) = a(ii) + (b(ii)-a(ii))*(1-z.^(1/beta(ii))).^(1/alpha(ii));
                    end
                end

                % Lower and upper bounded scalars with nonparametric CDF transform
                idx = trinfo.type == 7;
                if any(idx)
                    xspace = trinfo.xspace;
                    pspace = trinfo.pspace;
                    for ii = find(idx)
                        x(:,ii) = interp1(pspace(ii,:),xspace(ii,:),normcdf(y(:,ii)));
                    end
                end

                % Lower and upper bounded scalars with GMM CDF transform
                idx = trinfo.type == 8;
                if any(idx)
                    for ii = find(idx)
                        gmm = trinfo.gmm{ii};
                        z = normcdf(y(:,ii));
                        % z = logicdf(y(:,ii));
                        for j = 1:size(z,1)
                            z(j) = tgmminv(z(j),gmm);
                        end
                        x(:,ii) = a(ii) + (b(ii)-a(ii)).*z;
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logit transform
                idx = trinfo.type == 9;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        z = exp(-y(:,ii))./(1+exp(-y(:,ii)));   % 1 - logistic(z)
                        x(:,ii) = a(ii) + (b(ii)-a(ii))*(1-z.^(1/beta(ii))).^(1/alpha(ii));
                    end
                end

                % Unbounded scalars with logistic-Kumaraswamy-logit transform
                idx = trinfo.type == 10;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        % z = exp(-y(:,ii))./(1+exp(-y(:,ii)));   % 1 - logistic(z)
                        % % z = (1-z.^(1/beta(ii))).^(1/alpha(ii));
                        % z = 1./alpha(:,ii).*log1p(-z.^(1./beta(:,ii))) - log1p(-(1-z.^(1./beta(:,ii))).^(1./alpha(:,ii)));

                        % Small u (~ zero)
                        uzero = y(:,ii) < log(Tol);
                        u = 1./(1 + exp(-y(uzero,ii)));
                        %x(uzero,ii) = 1/alpha(ii)*(-log1p(exp(-y(uzero,ii)))-log(beta(ii))) - log1p(-(u/beta(ii)).^(1/alpha(ii)));
                        x(uzero,ii) = 1/alpha(ii)*(y(uzero,ii)-log(beta(ii))) - log1p(-(u/beta(ii)).^(1/alpha(ii)));

                        % Large u (~ one)
                        uone = y(:,ii) > -log(Tol);
                        w = 1./(1 + exp(y(uone,ii))).^(1/beta(ii));
                        x(uone,ii) = 1/alpha(ii)*log1p(-w) + 1/beta(ii)*y(uone,ii) + log(alpha(ii));

                        % All other cases
                        urest = ~uzero & ~uone;
                        z = 1./(1+exp(y(urest,ii)));
                        x(urest,ii) = 1./alpha(:,ii).*log1p(-z.^(1./beta(:,ii))) - log1p(-(1-z.^(1./beta(:,ii))).^(1./alpha(:,ii)));

                        x(:,ii) = x(:,ii).*delta(:,ii) + mu(:,ii);
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy inverse CDF transform
                idx = trinfo.type == 11;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    y(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    for ii = find(idx)
                        z = 1./(1+exp(-y(:,ii)));
                        x(:,ii) = a(ii) + (b(ii)-a(ii))*(1 - (1-z.^(alpha(ii))).^beta(ii));
                    end
                end

                % Lower and upper bounded scalars (cumulative normal)
                idx = trinfo.type == 12;
                if any(idx)
                    x(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    x(:,idx) = bsxfun(@plus, a(:,idx), bsxfun(@times, ...
                        b(idx)-a(idx), 0.5 * erfc(-x(:,idx) ./ sqrt(2))));
                end

                % Lower and upper bounded scalars (Student-t, nu = 4)
                idx = trinfo.type == 13;
                if any(idx)
                    x(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    t2 = x(:,idx).^2;
                    f = 0.5 + 3/8*x(:,idx)./sqrt(1 + t2/4).*(1 - t2./(1 + t2/4)/12);
                    x(:,idx) = bsxfun(@plus, a(:,idx), bsxfun(@times, ...
                        b(idx)-a(idx), f));
                end

                % Force to stay within bounds
                a(isfinite(a)) = a(isfinite(a)) + eps(a(isfinite(a)));
                b(isfinite(b)) = b(isfinite(b)) - eps(b(isfinite(b)));
                x = bsxfun(@min,bsxfun(@max,x,a),b);
                varargout{1} = x;

            %% PDF (OR LOG PDF) CORRECTION
            case {'p','l','g'}  % pdf (or log pdf) correction
                y = varargin{1};
                % Rescale input
                if ~isempty(scale); y = bsxfun(@times,y,scale); end

                % Rotate input
                if ~isempty(trinfo.R_mat); y = y*trinfo.R_mat'; end

                logpdf_flag = strcmpi(action(1),'l');
                if logpdf_flag
                    p = zeros(size(y,1),nvars);
                else
                    p = ones(size(y,1),nvars);
                end
                grad_flag = strcmpi(action(1),'g');

                a = trinfo.lb_orig;
                b = trinfo.ub_orig;
                mu = trinfo.mu;
                delta = trinfo.delta;

                % Unbounded scalars
                idx = trinfo.type == 0;
                if any(idx)
                    p(:,idx) = repmat(log(delta(idx)),[size(p,1),1]);
                end

                % Lower or upper bounded scalars
                idx = trinfo.type == 1 | trinfo.type == 2;
                if any(idx)
                    p(:,idx) = y(:,idx);
                end

                % Lower and upper bounded scalars
                idx = trinfo.type == 3;
                if any(idx)
                    y(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    z = -log1p(exp(-y(:,idx)));
                    p(:,idx) = bsxfun(@plus, log(b(idx)-a(idx)), -y(:,idx) + 2*z);
                    p(:,idx) = bsxfun(@plus, p(:,idx), log(delta(idx)));
                end

                % Lower and upper bounded scalars with Beta CDF transform
                idx = trinfo.type == 4;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        z = -log1p(exp(-y(:,ii)));
                        x = min(max(eps,betainv(1./(1+exp(-y(:,ii))),alpha(ii),beta(ii))),1-eps);
                        logbeta = (alpha(ii)-1)*log(x) + (beta(ii)-1)*log1p(-x) ...
                            + gammaln(alpha(ii)+beta(ii)) - gammaln(alpha(ii)) - gammaln(beta(ii));
                        p(:,ii) = log(b(ii)-a(ii)) -logbeta -y(:,ii) + 2*z;

                        if any(~isfinite(p))
                            fprintf('aaaa!');
                        end
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy CDF transform
                idx = trinfo.type == 5;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        y(:,ii) = bsxfun(@plus,bsxfun(@times,y(:,ii),delta(ii)),mu(ii));

                        z = -log1p(exp(-y(:,ii)));
                        % x = kumarinv(1./(1+exp(-y(:,ii))),alpha(ii),beta(ii));

                        %p = 1./(1+exp(-y(:,ii)));
                        %x = (-((-(p-1)).^(1/beta(ii))-1)).^(1/alpha(ii));
                        %log(-(p-1)) = -y(,::) - log1p(exp(-y(:,ii))));

                        %(1/beta(ii)).*log(-(p-1))

                        % x = min(max(eps,kumarinv(1./(1+exp(-y(:,ii))),alpha(ii),beta(ii))),1-eps);
                        u = 1./(1 + exp(-y(:,ii)));
                        logf = (1-1/alpha(ii))*log1p(-(1-u).^(1/beta(ii))) + (1-1/beta(ii))*(-y(:,ii)+z) ...
                            + log(alpha(ii)) + log(beta(ii));

                        % Special case for very small u
                        %idx_small = u < 1e12;
                        %logf(idx_small) = (1-1/alpha(ii))*(-y(idx_small,ii) + z - log(beta(ii))) + (1-1/beta(ii))*(-y(idx_small,ii)+z(idx_small)) ...
                        %    + log(alpha(ii)) + log(beta(ii));

                        %logf = (alpha(ii)-1)*log(x) + (beta(ii)-1)*log1p(-x.^alpha(ii)) ...
                        %    + log(alpha(ii)) + log(beta(ii));
                        p(:,ii) = log(b(ii)-a(ii)) -logf -y(:,ii) + 2*z;
                        p(:,ii) = bsxfun(@plus, p(:,ii), log(delta(ii)));

                        if any(~isfinite(p))
                            p(~isfinite(p)) = -Inf;
                            fprintf('aaaa!');
                        end
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logistic-power transform
                idx = trinfo.type == 6;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;
                        mu = trinfo.mu;
                        gamma = trinfo.gamma;

                        yl = sign(y(:,ii)).*abs(y(:,ii)).^(1/gamma(ii)) + mu(ii);

                        z = -log1p(exp(-yl));
                        u = 1./(1 + exp(-yl));
                        logf = (1-1/alpha(ii))*log1p(-(1-u).^(1/beta(ii))) + (1-1/beta(ii))*(-yl+z) ...
                            + log(alpha(ii)) + log(beta(ii));
                        logf = logf + log(gamma(ii)) + (gamma(ii)-1)*log(abs(yl-mu(ii)));
                        p(:,ii) = log(b(ii)-a(ii)) -logf -yl + 2*z;
                    end
                end

                % Lower and upper bounded scalars with nonparametric CDF transform
                idx = trinfo.type == 7;
                if any(idx)
                    xspace = trinfo.xspace;
                    pspace = trinfo.pspace;
                    for ii = find(idx)
                        z = -0.5*log(2*pi) -0.5*y(:,ii).^2;

                        yinv = normcdf(y(:,ii));
                        [~,pos] = histc(yinv,pspace(ii,:));
                        dx = [-log(diff(xspace(ii,:))),Inf];
                        logf = dx(pos);

                        p(:,ii) = log(b(ii)-a(ii)) -logf(:) + z;
                    end
                end

                % Lower and upper bounded scalars with GMM CDF transform
                idx = trinfo.type == 8;
                if any(idx)
                    for ii = find(idx)
                        gmm = trinfo.gmm{ii};

                        z = -0.5*log(2*pi) -0.5*y(:,ii).^2;
                        % z = -y(:,ii) - 2*log1p(exp(-y(:,ii)));

                        yinv = normcdf(y(:,ii));
                        for j = 1:size(yinv,1); yinv(j) = tgmminv(yinv(j),gmm); end

                        logf = log(gmm.lambda + (1-gmm.lambda)*gmm1pdf(yinv,gmm.w,gmm.Mu,gmm.Sigma)./gmm.Norm);

                        p(ii) = log(b(ii)-a(ii)) - logf + z;

                        if any(~isfinite(p))
                            p(~isfinite(p)) = -Inf;
                            fprintf('aaaa!');
                        end
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logit transform
                idx = trinfo.type == 9;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;
                        nf = (b(ii)-a(ii))/alpha(ii)/beta(ii);
                        k = 1./(1+exp(-y(:,ii)));

                        logk = -log1p(exp(-y(:,ii)));
                        log1mk = logk - y(:,ii);
                        logz = 1/alpha(ii)*log1p(-(1-k).^(1/beta(ii)));

                        % k small (close to zero)
                        kzero = k < sqrt(eps);
                        log1mk(kzero) = log1p(-k(kzero));
                        logz(kzero) = 1/alpha(ii) * (logk(kzero)-log(beta(ii))) + 1/alpha(ii)*log1p(0.5*(1-1/beta(ii)*k(kzero)));

                        %z = (1-(1-k).^(1/beta(ii))).^(1/alpha(ii));
                        % 1 - z^alpha = (1-k).^(1/beta(ii))
                        p(:,ii) = log(nf) + (1/beta(ii)-1) * log1mk + (1-alpha(ii))*logz -y(:,ii)+2*logk;
                    end
                end

                % Unbounded scalars with logistic-Kumaraswamy-logit transform
                idx = trinfo.type == 10;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        lnf = log(delta(:,ii)./alpha(:,ii)./beta(:,ii));

                        if 0

                            u = 1./(1+exp(y(:,ii)));
                            logu = -log1p(exp(y(:,ii)));
                            z = (1-u.^(1./beta(:,ii))).^(1./alpha(:,ii));
                            p(:,ii) = lnf + y(:,ii) + (1+1./beta(:,ii)).*logu - log1p(-u.^(1./beta(:,ii))) - log1p(-z);

                        else

                            p(:,ii) = lnf -y(:,ii) - 2*log1p(exp(-y(:,ii)));

                            % Small u (~ zero)
                            uzero = y(:,ii) < log(Tol);
                            u = 1./(1 + exp(-y(uzero,ii)));
                            % p(uzero,ii) = p(uzero,ii) - log(u/beta(ii)) + (1/beta(ii)-1)*log1p(-u);
                            p(uzero,ii) = p(uzero,ii) - y(uzero,ii) + log(beta(ii)) + (1/beta(ii)-1)*log1p(-u);
                            t = (1 - (1-u).^(1/beta(ii))).^(1/alpha(ii));
                            p(uzero,ii) = p(uzero,ii) - log1p(-t);

                            % Large u (~ one)
                            uone = y(:,ii) > -log(Tol);
                            w = 1./(1 + exp(y(uone,ii)));
                            p(uone,ii) = p(uone,ii) - log1p(-w.^(1/beta(ii))) + y(uone,ii) + log(alpha(ii));

                            % All other cases
                            urest = ~uzero & ~uone;
                            u = 1./(1 + exp(-y(urest,ii)));
                            p(urest,ii) = p(urest,ii) - log1p(-(1-u).^(1/beta(ii))) + (1/beta(ii)-1)*log1p(-u);
                            t = (1 - (1-u).^(1/beta(ii))).^(1/alpha(ii));
                            p(urest,ii) = p(urest,ii) - log1p(-t);

                        end

                    end
                end

                % Lower and upper bounded scalars with inverse Kumaraswamy logit CDF transform
                idx = trinfo.type == 11;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        y(:,ii) = bsxfun(@plus,bsxfun(@times,y(:,ii),delta(ii)),mu(ii));

                        y_large = y(:,ii) > -log(eps)/2;

                        z = 1./(1+exp(-y(:,ii)));
                        u = 1 - (1-z.^(alpha(ii))).^beta(ii);

                        u(y_large) = 1 - alpha(ii)*exp(-beta(ii)*y(y_large,ii))./(1+alpha(ii)*beta(ii)*exp(-y(y_large,ii)));

%                        z = -log1p(exp(-y(:,ii)));
                        %u = 1./(1 + exp(-y(:,ii)));
                        %u = a(ii) + (b(ii)-a(ii))*(1 - (1-u.^(alpha(ii))).^beta(ii));
%                        logf = log(alpha(ii)) + log(beta(ii)) + (alpha(ii)-1).*log(u) + (beta(ii)-1) .* log1p(-u.^alpha(ii));
                         logf = (1-1/alpha(ii))*log1p(-(1-u).^(1/beta(ii))) + (1-1/beta(ii))*log1p(-u) ...
                             + log(alpha(ii)) + log(beta(ii));
                         logf(y_large) = (1-1/alpha(ii))*log1p(-alpha(ii)*exp(-y(y_large,ii))./(1+alpha(ii)*exp(-y(y_large,ii)))) ...
                             + (1-1/beta(ii))*log(alpha(ii)*exp(-beta(ii)*y(y_large,ii))./(1+alpha(ii)*beta(ii)*exp(-y(y_large,ii)))) ...
                             + log(alpha(ii)) + log(beta(ii));

%                         logf = (1-1/alpha(ii))*log1p(-(1-u).^(1/beta(ii))) + (1-1/beta(ii))*(-y(:,ii)+z) ...
%                             + log(alpha(ii)) + log(beta(ii));

                        w = log1p(-z);
                        w(y_large) = -y(y_large,ii) - log1p(exp(-y(y_large,ii)));

                        p(:,ii) = log(b(ii)-a(ii)) + logf + log(z) + w; %-y(:,ii) + 2*z;
                        p(:,ii) = bsxfun(@plus, p(:,ii), log(delta(ii)));

                        if any(~isfinite(p))
                            p(~isfinite(p)) = -Inf;
                            fprintf('aaaa!');
                        end
                    end
                end

                % Lower and upper bounded scalars (cumulative normal)
                idx = trinfo.type == 12;
                if any(idx)
                    y(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    z = -0.5*log(2*pi) - 0.5*y(:,idx).^2;
                    p(:,idx) = bsxfun(@plus, log(b(idx)-a(idx)), z);
                    p(:,idx) = bsxfun(@plus, p(:,idx), log(delta(idx)));
                end

                % Lower and upper bounded scalars (Student-t, nu = 4)
                idx = trinfo.type == 13;
                if any(idx)
                    y(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    z = log(3/8) - 5/2*log1p(y(:,idx).^2/4);
                    p(:,idx) = bsxfun(@plus, log(b(idx)-a(idx)), z);
                    p(:,idx) = bsxfun(@plus, p(:,idx), log(delta(idx)));
                end

                %if ~isempty(trinfo.R_mat) && lower(action(1)) == 'g'
                %    p = p*(trinfo.R_mat*diag();
                %end

                % Scale transform
                if ~isempty(scale) && ~grad_flag
                    p = bsxfun(@plus,p,log(scale));
                end

                if ~grad_flag; p = sum(p,2); end
                if ~logpdf_flag; p = exp(p); end

                varargout{1} = p;

            %% FIRST DERIVATIVE WRT TRANSFORMATION PARAMETERS (ignores final rotation and scaling)
            case {'f'}
                y = varargin{1};
                nvars = numel(trinfo.lb_orig);

                % Rescale input
                if ~isempty(scale); y = bsxfun(@times,y,scale); end

                % Rotate input
                if ~isempty(trinfo.R_mat); y = y*trinfo.R_mat'; end

                a = trinfo.lb_orig;
                b = trinfo.ub_orig;
                delta = trinfo.delta;

                % Lower and upper bounded scalars with Kumaraswamy-logit transform
                % and unbounded scalars with logistic-Kumaraswamy-logit transform
                idx = (trinfo.type == 9 | trinfo.type == 10);
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        k = 1./(1+exp(-y(:,ii)));
                        talpha = 1 - (1-k).^(1/beta(ii));
                        logt = 1/alpha(ii) .* log1p(-(1-k).^(1/beta(ii)));

                        p(:,ii) = talpha.*beta(ii).*logt./(1-talpha)./k;
                        p(:,ii+nvars) = -log1p(-talpha)./k;
                    end
                end

                varargout{1} = p;

            %% MIXED DERIVATIVE WRT TRANSFORMATION PARAMETERS of FIRST DERIVATIVE
            % (ignores final rotation and scaling)
            case {'m'}
                y = varargin{1};
                nvars = numel(trinfo.lb_orig);

                % Rescale input
                if ~isempty(scale); y = bsxfun(@times,y,scale); end

                % Rotate input
                if ~isempty(trinfo.R_mat); y = y*trinfo.R_mat'; end

                a = trinfo.lb_orig;
                b = trinfo.ub_orig;
                delta = trinfo.delta;

                % Lower and upper bounded scalars with Kumaraswamy-logit transform
                % and unbounded scalars with logistic-Kumaraswamy-logit transform
                idx = (trinfo.type == 9 | trinfo.type == 10);
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        k = 1./(1+exp(-y(:,ii)));
                        talpha = 1 - (1-k).^(1/beta(ii));
                        logt = 1/alpha(ii) .* log1p(-(1-k).^(1/beta(ii)));
                        t = talpha.^(1/alpha(ii));

                        if trinfo.type(ii) == 9
                            nf = - 1./(b(ii) - a(ii)) ./ t;
                        else
                            nf = (t-1)./delta(ii);
                        end
                        den = 1 ./ k.^2 ./ (talpha-1) .* nf;
                        p(:,ii) = talpha.*beta(ii).*((-k.*(1+alpha(ii).*logt)) + talpha.*(1 + (1-k).*(-1+alpha(ii).*beta(ii).*logt))) ...
                            .* den ./ (talpha - 1);
                        p(:,ii+nvars) = talpha .* alpha(ii) .* (1 + (1-k).*(-1 - y(:,ii) - log1p(exp(-y(:,ii))))) ...
                            .* den;
                    end
                end

                varargout{1} = p;

            otherwise
                error(['Unkwnown transformation direction ''' action '''. Allowed values are direct (''dir'' or ''d'') and inverse (''inv'' or ''i'').']);
        end
    end

else
%% Create transform

    nvars = varargin{1};
    lb = varargin{2}(:)';
    ub = varargin{3}(:)';
    if nargin > 3
        plb = varargin{4}(:)';
        pub = varargin{5}(:)';
    else
        plb = []; pub = [];
    end
    if nargin > 5
        bounded_type = varargin{6};
    else
        % Default bounded type is logit
        bounded_type = 3;
    end

    % Empty LB and UB are Infs
    if isempty(lb); lb = -Inf; end
    if isempty(ub); ub = Inf; end

    % Empty plausible bounds equal hard bounds
    if isempty(plb); plb = lb; end
    if isempty(pub); pub = ub; end

    % Convert scalar inputs to row vectors
    if isscalar(lb); lb = lb*ones(1,nvars); end
    if isscalar(ub); ub = ub*ones(1,nvars); end
    if isscalar(plb); plb = plb*ones(1,nvars); end
    if isscalar(pub); pub = pub*ones(1,nvars); end

    % Check that the order of bounds is respected
    assert(all(lb <= plb & plb < pub & pub <= ub), ...
        'Variable bounds should be LB <= PLB < PUB <= UB for all variables.');

    % Transform to log coordinates
    trinfo.lb_orig = lb;
    trinfo.ub_orig = ub;

    trinfo.type = zeros(1,nvars);
    for i = 1:nvars
        if isfinite(lb(i)) && isinf(ub(i)); trinfo.type(i) = 1; end
        if isinf(lb(i)) && isfinite(ub(i)); trinfo.type(i) = 2; end
        if isfinite(lb(i)) && isfinite(ub(i)) && lb(i) < ub(i); trinfo.type(i) = bounded_type; end
    end

    % Centering (at the end of the transform)
    trinfo.mu = zeros(1,nvars);
    trinfo.delta = ones(1,nvars);

    % Get transformed PLB and PUB
    plb = warpvars_vbmc(plb,'d',trinfo);
    pub = warpvars_vbmc(pub,'d',trinfo);

    % Center in transformed space
    for i = 1:nvars
        if isfinite(plb(i)) && isfinite(pub(i))
            trinfo.mu(i) = 0.5*(plb(i)+pub(i));
            trinfo.delta(i) = (pub(i)-plb(i));
        end
    end

    varargout{1} = trinfo;

end

end

%--------------------------------------------------------------------------
function y = tgmminv(p,gmm)
    if p <= 0; p = eps; elseif p >= 1; p = 1 - eps; end    % Correct bounds
    fun = @(z) tgmmfzero(z,p,gmm.w,gmm.Mu,gmm.Sigma,gmm.Min,gmm.iMax,gmm.Norm,gmm.lambda);
    y = qfzero(fun,[-eps,1+eps]);
    if y <= 0; y = eps; elseif y >= 1; y = 1 - eps; end    % Correct bounds
end


function [err_N, sG] = ptv_nuclear_metric(volfix, voldef, pix_resolution, mask, singular_coefs, norm_type)
    volsz = [size(voldef, 1), size(voldef, 2), size(voldef, 3)];
    npix = prod(volsz);
    Nimgs = size(voldef, 5);

%     nuc_G = zeros(size(voldef), 'like', voldef);
    err_N = 0;

%     imagesc([voldef(:,:, round(end/2), 1, 1), volfix(:,:, round(end/2), 1, 1)]); pause(0.02);
%     imagesc([ mean(voldef(:,:, round(end/2), 1, :), 5)]); pause(0.02);

    if isempty(singular_coefs)
        singular_coefs = fl(sqrt(1:Nimgs));
        singular_coefs(1) = 0;
    end

    if isempty(volfix)
        X = reshape(voldef, [npix, Nimgs]);
    else
        X = reshape(cat(5, volfix, voldef), [npix, Nimgs+1]);
        if numel(singular_coefs > 1)
%             singular_coefs = [0, 1, singular_coefs(2:end)];
%             singular_coefs = [fl(singular_coefs(1:end)); singular_coefs(end)];
%             singular_coefs = fl(sqrt(1:(Nimgs+1)));
%             singular_coefs
        end
    end
    if ~isempty(mask)
        mask = logical(mask > 0.5);
        X = X(mask, :);
    end
    singular_coefs = singular_coefs(1 : min(numel(singular_coefs), size(X, 1)));
    [sU, sS, sV] = svdecon(X);
%     sS
    diagS = diag(sS);
    if false
        sGt = sU * ( (diag(singular_coefs)) * sV');
        err_N = sum(diagS(:) .* singular_coefs(:));
    else
        eps = 1e-4;
%         eps=0.1;
        G_sS = diagS ./ sqrt(diagS.^2 + eps);
        sGt = sU * ( (diag(singular_coefs(:) .* G_sS(:))) * sV');
        err_N = sum( sqrt(diagS.^2 + eps) .* singular_coefs(:));
    end

%     if true
%         sk1 = sqrt(1 : size(singular_coefs, 1));
%         sk2 = sk1;
%         sk2(1) = 0;
%
%         sk3 = ones(size(singular_coefs, 1), 1);
%         sk4 = sk3;
%         sk4(1) = 0;
%
%         size(sk1)
%         subplot(221);
%         itmp = reshape(sU * (diag(sk1) * sV'), [volsz, Nimgs]);
%         imagesc(itmp(:,:, 70)); colorbar;
%
%         subplot(222);
%         itmp = reshape(sU * (diag(sk2) * sV'), [volsz, Nimgs]);
%         imagesc(itmp(:,:, 70)); colorbar;
%
%         subplot(223);
%         itmp = reshape(sU * (diag(sk3) * sV'), [volsz, Nimgs]);
%         imagesc(itmp(:,:, 70)); colorbar;
%
%         subplot(224);
%         itmp = reshape(sU * (diag(sk4) * sV'), [volsz, Nimgs]);
%         imagesc(itmp(:,:, 70)); colorbar;
%
%         pause(0.05);
%         pause();
%     end

    if isempty(volfix)
        if ~isempty(mask)
            sG = zeros([npix, Nimgs], 'like', voldef);
            sG(mask, :) = reshape(sGt, [nnz(mask), Nimgs]);
            sG = reshape(sG, [volsz, Nimgs]);
        else
            sG = reshape(sGt, [volsz, Nimgs]);
        end
    else
        if ~isempty(mask)
            sG = zeros([npix, Nimgs+1], 'like', voldef);
            sG(mask, :) = reshape(sGt, [nnz(mask), Nimgs+1]);
            sG = reshape(sG, [volsz, Nimgs+1]);
        else
            sG = reshape(sGt, [volsz, Nimgs+1]);
        end
        sG = sG(:,:,:, 2:end);
    end

end


function d = mg_q1cd_supg(xy,ev,expe,eph,epw)
%mg_q1cd_supg  streamline diffusion matrix generator for GMG
%   d = mg_q1cd_supg(xy,ev,expe,eph,epw)
%   input
%           xy         vertex coordinate vector
%           ev         element mapping matrix
%           expe       element peclet numbers
%           eph        flow specific element lengths
%           epw        centroid evaluated wind
%   output
%           d          discrete streamline diffusion operator
%
%   IFISS function: DJS; 26 February 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

%   Analogous to femq1_cd_supg.m
x=xy(:,1); y=xy(:,2);
nvtx=length(x);
nel=length(ev(:,1));
%
% find the elements where streamline diffusion is active
acte = find(isfinite(expe)); nacte=length(acte);
%
% initialise global matrices
      d = sparse(nvtx,nvtx);
%
% set up 2x2 Gauss points
      gpt=1.0e0/sqrt(3.0e0);
      s(1) = -gpt;  t(1) = -gpt;
      s(2) =  gpt;  t(2) = -gpt;
      s(3) =  gpt;  t(3) =  gpt;
      s(4) = -gpt;  t(4) =  gpt;
%
% loop over active and inactive elements
%      for iact = 1:nacte
% ielem = acte(iact);
% inner loop over elements
        for ivtx = 1:4
        xl_v(:,ivtx) = x(ev(:,ivtx));
        yl_v(:,ivtx) = y(ev(:,ivtx));
		end
        de = zeros(nel,4,4);
% loop over 2x2 Gauss points
         for igpt = 1:4
         sigpt=s(igpt);
         tigpt=t(igpt);
%  evaluate derivatives etc
         [jac,invjac,phi,dphidx,dphidy] = deriv(sigpt,tigpt,xl_v,yl_v);
         [flowx,flowy] = gauss_transprt(sigpt,tigpt,xl_v,yl_v);
		 for j = 1:4
               for i = 1:4
    de(:,i,j) = de(:,i,j) + flowx(:).*dphidx(:,i).*flowx(:).*dphidx(:,j).*invjac(:);
    de(:,i,j) = de(:,i,j) + flowy(:).*dphidy(:,i).*flowx(:).*dphidx(:,j).*invjac(:);
    de(:,i,j) = de(:,i,j) + flowx(:).*dphidx(:,i).*flowy(:).*dphidy(:,j).*invjac(:);
    de(:,i,j) = de(:,i,j) + flowy(:).*dphidy(:,i).*flowy(:).*dphidy(:,j).*invjac(:);
               end
	    end
% end of Gauss point loop
         end
%
% scale with the appropriate parameter
      acte = find(isfinite(expe));
      factor = expe(acte); flow_h=eph(acte); flow_l2=epw(acte);
	  lpe =zeros(nel,1); lpe(acte)= factor.*(flow_h./flow_l2);
		 for j = 1:4
               for i = 1:4
               de(:,i,j) = lpe(:) .* de(:,i,j);
		   end
	   end
%
% perform assembly of global matrix  and source vector
      for krow=1:4
	  nrow=ev(:,krow);
          for kcol=1:4
		  ncol=ev(:,kcol);
          d = d + sparse(nrow,ncol,de(:,krow,kcol),nvtx,nvtx);
          end
      end
%
return


function [aVals,aJacob,aHess,papt]=aSpiralSimp(xPoints,numRetDims)
%%ASPIRALSIMP The drift function for a non-ballistic spiraling target
%         motion model in 3 dimensions, formulated in a manner such that it
%         is simple to make a spiraling target follow a nominal trajectory.
%         This model is probably bad to design into a target tracking
%         algorithm, but might be good for designing spiraling trajectories
%         to use to test target tracking algorithms.
%
%INPUTS: xPoints The target state at time t. It consists of position
%                (3 elements), instantaneous velocity (3 elements), the
%                velocity vector (3 elements) of the overall direction of
%                motion of the spiraling model (ground speed) and the
%                spiral rate of the target. Thus xDim=10. If x is an
%                xDim X numStates matrix, then the spiraling model is
%                evaluated for all of the state vectors. The last 4
%                elements in xPoints are taken to be constants and thus
%                aVal entries will be 0 for them. If only aVals is
%                requested on the output, then xPoints can be a matrix of
%                points.
%     numRetDims If numRetDims=6, then the last 4 elements in xPoints (the
%                velocity vector of the overall spiraling model and the
%                spiral rate) are taken to be constants and the returned
%                aVals Vec is 6X1. Otherwise, numRetDims can be 10 and
%                aVals is 10X1. The default if omitted or an empty matrix
%                is passed is 6.
%
%OUTPUTS: aVals The 6X1 (or 10X1 depending on numRetDims) flat-Earth time-
%               derivative of the state. If xPoints was a matrix of N
%               points, then this will be 6XN (or 10XN).
%        aJacob The 6X6 (or 10X10) matrix of partial derivatives of aVals
%               such that aJacob(:,k) is the partial derivative of
%               aVals(:,k) with respect to
%               xPoints(k). xPoints can't be a matrix if this output is
%               requested.
%         aHess The 6X6X6 (or 10X10X10) matrix of second derivatives of
%               aVals such that aHess(:,k1,k2) is the second partial
%               derivative of aVals with respect to xPoints(k1) and
%               xPoints(k2).
%          papt The 6X1 or 10X1 derivative with resect to time of aVals.
%               This is all zeros, because the model is time invariant.
%
%A derivation of the simplified flat-Earth spiraling dynamic model is given
%in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "Simulating aerial targets in 3D accounting for the
%    Earth's curvature," Journal of Advances in Information Fusion, vol.
%    10, no. 1, Jun. 2015.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<3||isempty(numRetDims))
        numRetDims=6;
    end

    if(numRetDims<6||numRetDims>10)
       error('numRetDims must be between 6 and 10.')
    end

    numPoints=size(xPoints,2);
    aVals=zeros(numRetDims,numPoints);

    vl=xPoints(7:9,:);
    omega=xPoints(10,:);

    vlMag2=sum(vl.*vl,1);
    vlMag=sqrt(vlMag2);

    aVals(1:3,:)=xPoints(4:6,:);
    vs=xPoints(4:6,:)-vl;
    ul=bsxfun(@rdivide,vl,vlMag);
    vsDot=omega.*bsxfun(@cross,ul,vs);
    aVals(4:6,:)=vsDot;

    if(nargout>1)
        rdotx=xPoints(4);
        rdoty=xPoints(5);
        rdotz=xPoints(6);

        vlx=vl(1);
        vly=vl(2);
        vlz=vl(3);

        vlx2=vlx*vlx;
        vly2=vly*vly;
        vlz2=vlz*vlz;

        vlMag2=vlMag*vlMag;
        vlMag3=vlMag2*vlMag;

        pXrdy=-(omega*vlz)/vlMag;
        pXrdz=(omega*vly)/vlMag;

        pYrdx=(omega*vlz)/vlMag;
        pYrdz=-((omega*vlx)/vlMag);

        pZrdx=-((omega*vly)/vlMag);
        pZrdy=(omega*vlx)/vlMag;

        if(numRetDims==6)
            aJacob=[0,0,0,1,        0,      0;
                    0,0,0,0,        1,      0;
                    0,0,0,0,        0,      1;
                    0,0,0,0,        pXrdy,  pXrdz;
                    0,0,0,pYrdx,    0,      pYrdz;
                    0,0,0,pZrdx,    pZrdy,  0];
        else
            pXvx=(omega*vlx*(-rdotz*vly+rdoty*vlz))/vlMag3;
            pXvy=(omega*(rdoty*vly*vlz+rdotz*(vlx2+vlz2)))/vlMag3;
            pXvz=-((omega*(rdoty*(vlx2+vly2)+rdotz*vly*vlz))/vlMag3);

            pXomega=(rdotz*vly-rdoty*vlz)/vlMag;

            pYvx=-((omega*(rdotx*vlx*vlz+rdotz*(vly2+vlz2)))/vlMag3);
            pYvy=(omega*vly*(rdotz*vlx-rdotx*vlz))/vlMag3;
            pYvz=(omega*(rdotx*(vlx2+vly2)+rdotz*vlx*vlz))/vlMag3;
            pYomega=(-rdotz*vlx+rdotx*vlz)/vlMag;

            pZvx=(omega*(rdotx*vlx*vly+rdoty*(vly2+vlz2)))/vlMag3;
            pZvy=-((omega*(rdoty*vlx*vly+rdotx*(vlx2+vlz2)))/vlMag3);
            pZvz=(omega*(-rdoty*vlx+rdotx*vly)*vlz)/vlMag3;
            pZomega=(rdoty*vlx-rdotx*vly)/vlMag;

            aJacob=[0,0,0,1,        0,      0,      0,      0,      0,      0;
                    0,0,0,0,        1,      0,      0,      0,      0,      0;
                    0,0,0,0,        0,      1,      0,      0,      0,      0;
                    0,0,0,0,        pXrdy,  pXrdz,  pXvx,   pXvy,   pXvz,   pXomega;
                    0,0,0,pYrdx,    0,      pYrdz,  pYvx,   pYvy,   pYvz,   pYomega;
                    0,0,0,pZrdx,    pZrdy,  0,      pZvx,   pZvy,   pZvz,   pZomega;
                    zeros(4,10)];
        end

        if(nargout>2)
            aHess=zeros(numRetDims,numRetDims,numRetDims);

            vlMag5=vlMag3*vlMag2;

            %Second deriavtives are all 0 if only
            if(numRetDims>6)
                pXrdyvx=(omega*vlx*vlz)/vlMag3;
                pXrdyvy=(omega*vly*vlz)/vlMag3;
                pXrdyvz=-((omega*(vlx2+vly2))/vlMag3);
                pXrdyOmega=-(vlz/vlMag);

                pXrdzvx=-((omega*vlx*vly)/vlMag3);
                pXrdzvy=(omega*(vlx2+vlz2))/vlMag3;
                pXrdzvz=-((omega*vly*vlz)/vlMag3);
                pXrdzOmega=vly/vlMag;

                pXvxrdx=0;
                pXvxrdy=pXrdyvx;
                pXvxrdz=pXrdzvx;
                pXvxvx=-((omega*(rdotz*vly-rdoty*vlz)*(-2*vlx2+vly2+vlz2))/vlMag5);
                pXvxvy=-((omega*vlx*(3*rdoty*vly*vlz+rdotz*(vlx2-2*vly2+vlz2)))/vlMag5);
                pXvxvz=(omega*vlx*(3*rdotz*vly*vlz+rdoty*(vlx2+vly2-2*vlz2)))/vlMag5;
                pXvxOmega=(-rdotz*vlx*vly+rdoty*vlx*vlz)/vlMag3;

                pXvyrdx=0;
                pXvyrdy=pXrdyvy;
                pXvyrdz=pXrdzvy;
                pXvyvx=pXvxvy;
                pXvyvy=(omega*(-3*rdotz*vly*(vlx2+vlz2)+rdoty*vlz*(vlx2-2*vly2+vlz2)))/vlMag5;
                pXvyvz=(omega*(rdoty*vly*(vlx2+vly2-2*vlz2)-rdotz*vlz*(vlx2-2*vly2+vlz2)))/vlMag5;
                pXvyOmega=(rdoty*vly*vlz+rdotz*(vlx2+vlz2))/vlMag3;

                pXvzrdx=0;
                pXvzrdy=pXrdyvz;
                pXvzrdz=pXrdzvz;
                pXvzvx=pXvxvz;
                pXvzvy=pXvyvz;
                pXvzvz=(omega*(3*rdoty*(vlx2+vly2)*vlz-rdotz*vly*(vlx2+vly2-2*vlz2)))/vlMag5;
                pXvzOmega=(-rdoty*(vlx2+vly2)-rdotz*vly*vlz)/vlMag3;

                pXOmegardx=0;
                pXOmegardy=pXrdyOmega;
                pXOmegardz=pXrdzOmega;
                pXOmegavx=pXvxOmega;
                pXOmegavy=pXvyOmega;
                pXOmegavz=pXvzOmega;
                pXOmegaOmega=0;

                pYrdxvx=-((omega*vlx*vlz)/vlMag3);
                pYrdxvy=-((omega*vly*vlz)/vlMag3);
                pYrdxvz=(omega*(vlx2+vly2))/vlMag3;
                pYrdxOmega=vlz/vlMag;

                pYrdzvx=-((omega*(vly2+vlz2))/vlMag3);
                pYrdzvy=(omega*vlx*vly)/vlMag3;
                pYrdzvz=(omega*vlx*vlz)/vlMag3;
                pYrdzOmega=-(vlx/vlMag);

                pYvxrdx=pYrdxvx;
                pYvxrdy=0;
                pYvxrdz=pYrdzvx;
                pYvxvx=(omega*(3*rdotz*vlx*(vly2+vlz2)-rdotx*vlz*(-2*vlx2+vly2+vlz2)))/vlMag5;
                pYvxvy=(omega*vly*(3*rdotx*vlx*vlz+rdotz*(-2*vlx2+vly2+vlz2)))/vlMag5;
                pYvxvz=(omega*(-rdotx*vlx*(vlx2+vly2-2*vlz2)+rdotz*vlz*(-2*vlx2+vly2+vlz2)))/vlMag5;
                pYvxOmega=(-rdotx*vlx*vlz-rdotz*(vly2+vlz2))/vlMag3;

                pYvyrdx=pYrdxvy;
                pYvyrdy=0;
                pYvyrdz=pYrdzvy;
                pYvyvx=pYvxvy;
                pYvyvy=(omega*(rdotz*vlx-rdotx*vlz)*(vlx2-2*vly2+vlz2))/vlMag5;
                pYvyvz=-((omega*vly*(3*rdotz*vlx*vlz+rdotx*(vlx2+vly2-2*vlz2)))/vlMag5);
                pYvyOmega=(vly*(rdotz*vlx-rdotx*vlz))/vlMag3;

                pYvzrdx=pYrdxvz;
                pYvzrdy=0;
                pYvzrdz=pYrdzvz;
                pYvzvx=pYvxvz;
                pYvzvy=pYvyvz;
                pYvzvz=(omega*(-3*rdotx*(vlx2+vly2)*vlz+rdotz*vlx*(vlx2+vly2-2*vlz2)))/vlMag5;
                pYvzOmega=(rdotx*(vlx2+vly2)+rdotz*vlx*vlz)/vlMag3;

                pYOmegardx=pYrdxOmega;
                pYOmegardy=0;
                pYOmegardz=pYrdzOmega;
                pYOmegavx=pYvxOmega;
                pYOmegavy=pYvyOmega;
                pYOmegavz=pYvzOmega;
                pYOmegaOmega=0;

                pZrdxvx=(omega*vlx*vly)/vlMag3;
                pZrdxvy=-((omega*(vlx2+vlz2))/vlMag3);
                pZrdxvz=(omega*vly*vlz)/vlMag3;
                pZrdxOmega=-(vly/vlMag);

                pZrdyvx=(omega*(vly2+vlz2))/vlMag3;
                pZrdyvy=-((omega*vlx*vly)/vlMag3);
                pZrdyvz=-((omega*vlx*vlz)/vlMag3);
                pZrdyOmega=vlx/vlMag;

                pZvxrdx=pZrdxvx;
                pZvxrdy=pZrdyvx;
                pZvxrdz=0;
                pZvxvx=(omega*(-3*rdoty*vlx*(vly2+vlz2)+rdotx*vly*(-2*vlx2+vly2+vlz2)))/vlMag5;
                pZvxvy=(omega*(rdotx*vlx*(vlx2-2*vly2+vlz2)-rdoty*vly*(-2*vlx2+vly2+vlz2)))/vlMag5;
                pZvxvz=-((omega*vlz*(3*rdotx*vlx*vly+rdoty*(-2*vlx2+vly2+vlz2)))/vlMag5);
                pZvxOmega=(rdotx*vlx*vly+rdoty*(vly2+vlz2))/vlMag3;

                pZvyrdx=pZrdxvy;
                pZvyrdy=pZrdyvy;
                pZvyrdz=0;
                pZvyvx=pZvxvy;
                pZvyvy=(omega*(3*rdotx*vly*(vlx2+vlz2)-rdoty*vlx*(vlx2-2*vly2+vlz2)))/vlMag5;
                pZvyvz=(omega*vlz*(3*rdoty*vlx*vly+rdotx*(vlx2-2*vly2+vlz2)))/vlMag5;
                pZvyOmega=(-rdoty*vlx*vly-rdotx*(vlx2+vlz2))/vlMag3;

                pZvzrdx=pZrdxvz;
                pZvzrdy=pZrdyvz;
                pZvzrdz=0;
                pZvzvx=pZvxvz;
                pZvzvy=pZvyvz;
                pZvzvz=-((omega*(rdoty*vlx-rdotx*vly)*(vlx2+vly2-2*vlz2))/vlMag5);
                pZvzOmega=(-rdoty*vlx*vlz+rdotx*vly*vlz)/vlMag3;

                pZOmegardx=pZrdxOmega;
                pZOmegardy=pZrdyOmega;
                pZOmegardz=0;
                pZOmegavx=pZvxOmega;
                pZOmegavy=pZvyOmega;
                pZOmegavz=pZvzOmega;
                pZOmegaOmega=0;

                aHess(:,:,4)=[zeros(3,10);
                              0,0,0,0,0,0,  pXvxrdx,   pXvyrdx,   pXvzrdx,   pXOmegardx;
                              0,0,0,0,0,0,  pYvxrdx,   pYvyrdx,   pYvzrdx,   pYOmegardx;
                              0,0,0,0,0,0,  pZvxrdx,   pZvyrdx,   pZvzrdx,   pZOmegardx;
                              zeros(4,10)];
                aHess(:,:,5)=[zeros(3,10);
                              0,0,0,0,0,0,  pXvxrdy,   pXvyrdy,   pXvzrdy,   pXOmegardy;
                              0,0,0,0,0,0,  pYvxrdy,   pYvyrdy,   pYvzrdy,   pYOmegardy;
                              0,0,0,0,0,0,  pZvxrdy,   pZvyrdy,   pZvzrdy,   pZOmegardy;
                              zeros(4,10)];
                aHess(:,:,6)=[zeros(3,10);
                              0,0,0,0,0,0,  pXvxrdz,   pXvyrdz,   pXvzrdz,   pXOmegardz;
                              0,0,0,0,0,0,  pYvxrdz,   pYvyrdz,   pYvzrdz,   pYOmegardz;
                              0,0,0,0,0,0,  pZvxrdz,   pZvyrdz,   pZvzrdz,   pZOmegardz;
                              zeros(4,10)];
                aHess(:,:,7)=[zeros(3,10);
                              0,0,0,0,          pXrdyvx,  pXrdzvx,  pXvxvx,   pXvyvx,   pXvzvx,   pXOmegavx;
                              0,0,0,pYrdxvx,    0,        pYrdzvx,  pYvxvx,   pYvyvx,   pYvzvx,   pYOmegavx;
                              0,0,0,pZrdxvx,    pZrdyvx,  0,        pZvxvx,   pZvyvx,   pZvzvx,   pZOmegavx;
                              zeros(4,10)];
                aHess(:,:,8)=[zeros(3,10);
                              0,0,0,0,          pXrdyvy,  pXrdzvy,  pXvxvy,   pXvyvy,   pXvzvy,   pXOmegavy;
                              0,0,0,pYrdxvy,    0,        pYrdzvy,  pYvxvy,   pYvyvy,   pYvzvy,   pYOmegavy;
                              0,0,0,pZrdxvy,    pZrdyvy,  0,        pZvxvy,   pZvyvy,   pZvzvy,   pZOmegavy;
                              zeros(4,10)];
                aHess(:,:,9)=[zeros(3,10);
                              0,0,0,0,          pXrdyvz,  pXrdzvz,  pXvxvz,   pXvyvz,   pXvzvz,   pXOmegavz;
                              0,0,0,pYrdxvz,    0,        pYrdzvz,  pYvxvz,   pYvyvz,   pYvzvz,   pYOmegavz;
                              0,0,0,pZrdxvz,    pZrdyvz,  0,        pZvxvz,   pZvyvz,   pZvzvz,   pZOmegavz;
                              zeros(4,10)];
               aHess(:,:,10)=[zeros(3,10);
                              0,0,0,0,             pXrdyOmega,  pXrdzOmega,  pXvxOmega,   pXvyOmega,   pXvzOmega,   pXOmegaOmega;
                              0,0,0,pYrdxOmega,    0,           pYrdzOmega,  pYvxOmega,   pYvyOmega,   pYvzOmega,   pYOmegaOmega;
                              0,0,0,pZrdxOmega,    pZrdyOmega,  0,           pZvxOmega,   pZvyOmega,   pZvzOmega,   pZOmegaOmega;
                              zeros(4,10)];
            end
            if(nargout>3)
                papt=zeros(10,1);
            end
        end
    end
end

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


function y = sinh(x)
%SINH         Implements  sinh(x)  for intervals
%
%   y = sinh(x)
%
%interval standard function implementation
%

% written  12/30/98     S.M. Rump
% modified 08/31/99     S.M. Rump  complex allowed, sparse input,
%                                  major revision, improved accuracy
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 12/04/05     S.M. Rump  extreme values for approximate part
% modified 09/06/07     S.M. Rump  approximate std fcts removed
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

  if issparse(x)
    [ix,jx,sx] = find(x);
    [m,n] = size(x);
    y = sparse(ix,jx,sinh(full(sx)),m,n);
    return
  end

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

  if x.complex
    y = ( exp(x) - exp(-x) ) / 2;
    setround(rndold)
    return
  end

  y = x;

  xinf = x.inf(:);
  xsup = x.sup(:);

  IndexInfPos = ( xinf>=0 );
  len1 = sum(IndexInfPos);
  IndexSupNeg = ( xsup<=0 );
  len2 = sum(IndexSupNeg);

  Y = sinhpos( [ xinf(IndexInfPos) ; -xsup(IndexSupNeg) ] , -1 );
  y.inf(IndexInfPos) = Y(1:len1);
  y.sup(IndexSupNeg) = -Y( len1+1 : end );

  IndexInfNeg = ~IndexInfPos;
  len1 = sum(IndexInfNeg);
  IndexSupPos = ~IndexSupNeg;
  len2 = sum(IndexSupPos);

  Y = sinhpos( [ -xinf(IndexInfNeg) ; xsup(IndexSupPos) ] , 1 );
  y.inf(IndexInfNeg) = -Y(1:len1);
  y.sup(IndexSupPos) = Y( len1+1 : len1+len2 );

  setround(rndold)

function y = sinhpos(x,rnd)
% rigorous sinh(x) for nonnegative double vector x with
% rounding corresponding to rnd

  INTLAB_STDFCTS_SINH = getappdata(0,'INTLAB_STDFCTS_SINH');

  y = x;

  % small input
  index = ( x<8 );
  if any(index)
    setround(0)
    xx = x(index);
    [f,e] = log2(xx);
    bits = 14 + min(e,0);
    xs = pow2( floor(f.*2.^bits) , e-bits );  % max. 14 bits of mantissa,
                                              % no bit below 2^-14
    d = xx - xs;                              % 0 <= d < 2^-14*x < 2^-11
    sinhxs = sinh(xs);                        % round to nearest

    % bounds for exp(xs)
    INTLAB_STDFCTS_EXP = getappdata(0,'INTLAB_STDFCTS_EXP');
    setround(-1)
    expinf = exp(xs)*(1-INTLAB_STDFCTS_EXP.EPS);
    setround(1)
    expsup = exp(xs)*(1+INTLAB_STDFCTS_EXP.EPS);
    expinf(xs==0) = 1;                    % gives accuracy for small x
    expsup(xs==0) = 1;

    % use sinh(xx) = sinh(xs+d) = sinh(xs)*cosh(d) + cosh(xs)*sinh(d)
    setround(rnd)
    corr = INTLAB_STDFCTS_SINH.EPS;
    if rnd==-1
      % coshxs <= cosh(xs)
      coshxs = 0.5*( expinf + 1./expsup );  % rounded downwards
      dd = d.*d;
      % 0 <= err(sinhd) <= sinh(d)*d^4/4! < 6e-8*d^3/6
      % 0 <= err(coshd) <= sinh(d)*d^5/5! < 4.8e-8*d^4/24
      y(index) = sinhxs + ...
          ( ( sinhxs .* ( -corr + (1-corr)*dd/2.*( 1 + dd/12 ) ) + ...
              coshxs.*d.*dd/6 ) + ...
            coshxs.*d ...
          );
    else
      % coshxs >= cosh(xs)
      coshxs = 0.5*( expsup + 1./expinf );  % correctly rounded according to rnd
      dd = d.*d;
      y(index) = sinhxs + ...
          ( ( sinhxs .* ( corr + (1+corr)*dd/2.*( 1 + dd/12.*( 1+4.8e-8 ) ) ) + ...
              coshxs.*d.*dd/6.*( 1+6e-8 ) ) + ...
            coshxs.*d ...
          );
    end
  end

  % medium input
  index = ( ~index ) & ( x<709 );         % 8 <= x < 709
  if any(index)
    xx = x(index);
    exprnd = exp_rnd(xx,rnd);
    setround(rnd)
    y(index) = 0.5 * ( exprnd + (-1)./exprnd );
  end

  % large input
  index = ( x>=709 );
  if any(index)
    INTLAB_STDFCTS_E = getappdata(0,'INTLAB_STDFCTS_E');
    exprnd = exp_rnd( x(index)-1 , rnd )/2;
    setround(rnd)
    if rnd==-1
      y(index) = exprnd .* INTLAB_STDFCTS_E.INF;
    else
      y(index) = exprnd .* INTLAB_STDFCTS_E.SUP;
    end
  end


%FIGURE_sliding_distance

%%%% EXPERIMENT %%%%
%
% For a slendar rod toppling from rest, plot how the slip distance varies
% as a function of coefficient of friction.
%
%

clc; clear;

setup.Tspan = [0,10];  %only used for timeout of the simulation
setup.tol = 1e-12;   %Accuracy of the intergation method
setup.dataFreq = 750;   %How much data to return?
setup.solver = @ode45;   %Tell the simulation to use a still solver.

%%%% HACK %%%%
% If the perturbation is too small, then numerical errors start to creep
% into the results. If perturbation is larger than the critical angle of
% the smallest non-zero coefficient of friction, then it will cause a
% direct error by violating the initial phase assumption.
setup.perturbation = 1e-2;
%%%% DONE %%%%

MoI = logspace(-3,0,250);
Mu = [0,0.05,0.1,0.5,1,5,10,inf];
P.m = 1;
P.g = 1;
P.L = 1;

%%%% Run big experiment %%%%
Data = runSlipExperiment(setup,MoI,Mu);

%%%% Plotting %%%%
N_mu = length(Data);
xBnd = [min(Data(1).moi), max(Data(1).moi)];
Mu = zeros(N_mu,1);
style = {'k--','b--','r--','m--','g--',...
    'k-','b-','r-','m-','g-'...
    'k:','b:','r:','m:','g:'};
LINEWIDTH = 3;
FontSize.Title = 16;
FontSize.label = 12;
names = cell(1,N_mu);
for i=1:N_mu
    Mu(i) = Data(i).mu;
    names{i} = num2str(['u = ' num2str(Mu(i))]);
end

%%%% Distance %%%%
H_dist = figure(100); clf; hold on;
set(H_dist,'Name','SlipDist','NumberTitle','off')
IDX = false(2,length(Mu));
for i=1:length(Data)
    sty = style{mod(i-1,length(style))+1};
    subplot(2,1,1); hold on;
    idx = Data(i).pos~=0;  IDX(1,i) = sum(idx)~=0;
    semilogx(Data(i).moi(idx),Data(i).pos(idx),sty,'LineWidth',LINEWIDTH);
    title('Backwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Slip Distance','FontSize',FontSize.label)
    set(gca,'Xscale','log')
    subplot(2,1,2); hold on;
    idx = Data(i).neg~=0;  IDX(2,i) = sum(idx)~=0;
    semilogx(Data(i).moi(idx),Data(i).neg(idx),sty,'LineWidth',LINEWIDTH);
    title('Forwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Slip Distance','FontSize',FontSize.label)
    set(gca,'Xscale','log')
end
%subplot(2,1,1); legend(names(IDX(1,:)),'Location','NorthEast');
% extents = [xBnd,-1,0];  axis(extents);
subplot(2,1,2); legend(names(IDX(2,:)),'Location','SouthWest');
% extents = [xBnd,0,0.14];

%%%% Critical Angle %%%%
H_angle = figure(101); clf; hold on;
set(H_angle,'Name','CriticalAngle','NumberTitle','off')
IDX = false(2,length(Mu));
for i=1:length(Data)
    sty = style{mod(i-1,length(style))+1};

    idx1 = Data(i).pos~=0;  IDX(1,i) = sum(idx1)~=0;
    subplot(2,1,1); hold on
    semilogx(Data(i).moi(idx1),-Data(i).angle(idx1)*180/pi,sty,'LineWidth',LINEWIDTH);
    title('Backwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Critical Angle (deg)','FontSize',FontSize.label)
    set(gca,'Xscale','log')

    idx2 = Data(i).neg~=0; IDX(2,i) = sum(idx2)~=0;
    idx2a = idx2 & idx1;   %In these special cases, it slips backwards and then forwards. Only plot the FIRST critical angle.
    idx2b = idx2 & ~idx1;
    subplot(2,1,2); hold on;
%     semilogx(Data(i).moi(idx2a),-Data(i).angle(idx2a)*180/pi,sty,'LineWidth',LINEWIDTH);
    semilogx(Data(i).moi(idx2b),-Data(i).angle(idx2b)*180/pi,sty,'LineWidth',LINEWIDTH);
    title('Forwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Critical Angle (deg)','FontSize',FontSize.label)
    set(gca,'Xscale','log')
end

subplot(2,1,1); legend(names(IDX(1,:)),'Location','NorthEast');
% extents = [xBnd,0,45];  axis(extents);
subplot(2,1,2); legend(names(IDX(2,:)),'Location','NorthWest');
% extents = [xBnd,30,60];

%%%% SAVE %%%%
save('DATA_Slip_vs_Mu.mat','Data');
save2pdf('../WriteUp/Figures/Slip_vs_Mu__Distance.pdf',H_dist,600);
save2pdf('../WriteUp/Figures/Slip_vs_Mu__Angle.pdf',H_angle,600);