function [km3] = l2km3(l)
% Convert volume from liters to cubic kilometers.
% Chad Greene 2012
km3 = l*1e-12;


% predicting numerical values using Linear Regression
clear all;
format long
disp('===== Linear Regression ====');
disp('Reading featur vector');

Indices = crossvalind('Kfold', 26548, 10);
figure;

subplot(3,2,1);

for feat = 1:3
      MSEarray =[];
    elapsedarray =[];
    for crossvalidateIter = 1:10
        (fprintf('%d',crossvalidateIter));
    featurs = csvread('data\forWeka_featuresonly.csv');
    num_data = size(featurs,1); %5000;
    %disp(sprintf('Number of datapoints %d',num_data))

    possiblefeaturizations =  {'bernouli', 'tfidf','multinomial'};
    %featurization = 'bernouli'%'tfidf'%'tfidf'%'multinomial'%'tfidf' %'multinomial'; % 'bernouli', 'tfidf'
    featurization  = possiblefeaturizations{feat};

    featurs = featurs(:,2:size(featurs,2));
    if strcmp(featurization,'multinomial')
        %just pass
    elseif strcmp(featurization,'bernouli')
        featurs = bernoulli(featurs);
    elseif strcmp(featurization,'tfidf')
        featurs = tfidf(featurs);
    end

     size_training = floor(.9*num_data);

        trainingset = featurs(Indices~=crossvalidateIter,:);
        testset = featurs(Indices==crossvalidateIter,:);

    %disp('Splitting up data into training/test sets');
    [num,txt,raw] = xlsread('data\final104.xls');

    % reading the description of each shoe
    descriptions = raw(2:size(raw,1),2);
    style_ratings = num(1:size(num,1),1);
    comfort_ratings = num(1:size(num,1),4);
    overal_ratings = num(1:size(num,1),5);

    % only take m data points
    m=num_data;
    descriptions = descriptions(1:m);
    style_ratings = style_ratings(1:m);
    comfort_ratings = comfort_ratings(1:m);
    overal_ratings = overal_ratings(1:m);

    responsevals = [style_ratings, comfort_ratings, overal_ratings];

        responsevals_training = responsevals(Indices~=crossvalidateIter,:);
        responsevals_test = responsevals(Indices==crossvalidateIter,:);

    %disp('Linear Regression');
    %

    tic;

    predictions = [];
    actual = [];
    for i =1:3
        a = responsevals_training(:,i);
        b = responsevals_test(:,i)';
        regresscoeff = regress(a, trainingset);
        C2 = (regresscoeff'*(testset'));
        predictions = [predictions, C2'];
        actual = [actual, b'];
    end

    MSE = mean(sum(((predictions-actual).^2)'))
     elapsed = toc;
        MSEarray = [MSEarray MSE];
        elapsedarray = [elapsedarray elapsed];

    end
    featurization  = possiblefeaturizations{feat}
    fprintf('Avergae MSE for Linear Regression is %0.10f\n', mean(MSEarray));
    subplot(3,2,feat*2-1)
    plot(MSEarray,'-o');
    title(strcat('MSE for Linear Regression (',featurization,') ', sprintf(' avergae MSE = %0.10f\n', mean(MSEarray))));
    xlabel('10 fold cross-validation (iteration no)')
    ylabel('MSE')
    subplot(3,2,feat*2)
    plot(elapsedarray,'-o');
    title(strcat('Elapsed time for Linear Regression (',featurization, ') ' , sprintf(' avergae elapsed time = %0.10f\n', mean(elapsedarray))));
    xlabel('10 fold cross-validation (iteration no)')
    ylabel('Elapsed Time')
    drawnow;
end


function [ H_output ] = homography_linearization( H, point, anchor_points )
vega = 5;

% Obtain kernel: Student’s t-weighting
alpha = (1 + pdist2(point,anchor_points)./vega).^(-(vega+1)/2);
alpha = alpha./sum(alpha);

% Linearization using Taylor series
H_output = zeros(3,3);

for i = 1:size(anchor_points,1)
    A = taylor_series( H, anchor_points(i,:));

    H_output = H_output + alpha(i).*A;
end

end

function [ A ] = taylor_series( H, p)
% The first two terms of Taylor series provide the best linearization of H
% H:3*3    p:1*2

h1 = H(1,1); h2 = H(1,2); h3 = H(1,3);
h4 = H(2,1); h5 = H(2,2); h6 = H(2,3);
h7 = H(3,1); h8 = H(3,2); h9 = H(3,3);
x1 = p(1,1);x2 = p(1,2);

dy1dx1 = h1/(h9 + h7*x1 + h8*x2) - (h7*(h3 + h1*x1 + h2*x2))/(h9 + h7*x1 + h8*x2)^2;
dy1dx2 = h2/(h9 + h7*x1 + h8*x2) - (h8*(h3 + h1*x1 + h2*x2))/(h9 + h7*x1 + h8*x2)^2;
dy2dx1 = h4/(h9 + h7*x1 + h8*x2) - (h7*(h6 + h4*x1 + h5*x2))/(h9 + h7*x1 + h8*x2)^2;
dy2dx2 = h5/(h9 + h7*x1 + h8*x2) - (h8*(h6 + h4*x1 + h5*x2))/(h9 + h7*x1 + h8*x2)^2;

y1 = (h3 + h1*x1 + h2*x2)/(h9 + h7*x1 + h8*x2);
y2 = (h6 + h4*x1 + h5*x2)/(h9 + h7*x1 + h8*x2);

A = [dy1dx1 dy1dx2 (y1 - x1*dy1dx1 - x2*dy1dx2);
     dy2dx1 dy2dx2 (y2 - x1*dy2dx1 - x2*dy2dx2);
          0      0                           1];
end


function M = grassmannfactory(n, p, k)
% Returns a manifold struct to optimize over the space of vector subspaces.
%
% function M = grassmannfactory(n, p)
% function M = grassmannfactory(n, p, k)
%
% Grassmann manifold: each point on this manifold is a collection of k
% vector subspaces of dimension p embedded in R^n.
%
% The metric is obtained by making the Grassmannian a Riemannian quotient
% manifold of the Stiefel manifold, i.e., the manifold of orthonormal
% matrices, itself endowed with a metric by making it a Riemannian
% submanifold of the Euclidean space, endowed with the usual inner product.
% In short: it is the usual metric used in most cases.
%
% This structure deals with matrices X of size n x p x k (or n x p if
% k = 1, which is the default) such that each n x p matrix is orthonormal,
% i.e., X'*X = eye(p) if k = 1, or X(:, :, i)' * X(:, :, i) = eye(p) for
% i = 1 : k if k > 1. Each n x p matrix is a numerical representation of
% the vector subspace its columns span.
%
% By default, k = 1.
%
% See also: stiefelfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%   March 22, 2013 (NB) : Implemented geodesic distance.
%   April 17, 2013 (NB) : Retraction changed to the polar decomposition, so
%                         that the vector transport is now correct, in the
%                         sense that it is compatible with the retraction,
%                         i.e., transporting a tangent vector G from U to V
%                         where V = Retr(U, H) will give Z, and
%                         transporting GQ from UQ to VQ will give ZQ: there
%                         is no dependence on the representation, which is
%                         as it should be. Notice that the polar
%                         factorization requires an SVD whereas the qfactor
%                         retraction requires a QR decomposition, which is
%                         cheaper. Hence, if the retraction happens to be a
%                         bottleneck in your application and you are not
%                         using vector transports, you may want to replace
%                         the retraction with a qfactor.
%   July  4, 2013 (NB)  : Added support for the logarithmic map 'log'.
%   July  5, 2013 (NB)  : Added support for ehess2rhess.
%   June 24, 2014 (NB)  : Small bug fix in the retraction, and added final
%                         re-orthonormalization at the end of the
%                         exponential map. This follows discussions on the
%                         forum where it appeared there is a significant
%                         loss in orthonormality without that extra step.
%                         Also changed the randvec function so that it now
%                         returns a globally normalized vector, not a
%                         vector where each component is normalized (this
%                         only matters if k>1).

    assert(n >= p, ...
           ['The dimension n of the ambient space must be larger ' ...
	        'than the dimension p of the subspaces.']);

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

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

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

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

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

    M.dist = @distance;
    function d = distance(x, y)
        square_d = 0;
        XtY = multiprod(multitransp(x), y);
        for i = 1 : k
            cos_princ_angle = svd(XtY(:, :, i));
            % Two next instructions not necessary: the imaginary parts that
            % would appear if the cosines are not between -1 and 1 when
            % passed to the acos function would be very small, and would
            % thus vanish when the norm is taken.
            % cos_princ_angle = min(cos_princ_angle,  1);
            % cos_princ_angle = max(cos_princ_angle, -1);
            square_d = square_d + norm(acos(cos_princ_angle))^2;
        end
        d = sqrt(square_d);
    end

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

    % Orthogonal projection of an ambient vector U to the horizontal space
    % at X.
    M.proj = @projection;
    function Up = projection(X, U)

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

    end

    M.tangent = M.proj;

	M.egrad2rgrad = M.proj;

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

    M.retr = @retraction;
    function Y = retraction(X, U, t)
        if nargin < 3
            t = 1.0;
        end
        Y = X + t*U;
        for i = 1 : k
            % We do not need to worry about flipping signs of columns here,
            % since only the column space is important, not the actual
            % columns. Compare this with the Stiefel manifold.
            % [Q, unused] = qr(Y(:, :, i), 0); %#ok
            % Y(:, :, i) = Q;

            % Compute the polar factorization of Y = X+tU
            [u, s, v] = svd(Y(:, :, i), 'econ'); %#ok
            Y(:, :, i) = u*v';
        end
    end

    M.exp = @exponential;
    function Y = exponential(X, U, t)
        if nargin == 3
            tU = t*U;
        else
            tU = U;
        end
        Y = zeros(size(X));
        for i = 1 : k
            [u s v] = svd(tU(:, :, i), 0);
            cos_s = diag(cos(diag(s)));
            sin_s = diag(sin(diag(s)));
            Y(:, :, i) = X(:, :, i)*v*cos_s*v' + u*sin_s*v';
            % From numerical experiments, it seems necessary to
            % re-orthonormalize. This is overall quite expensive.
            [q, unused] = qr(Y(:, :, i), 0); %#ok
            Y(:, :, i) = q;
        end
    end

    % Test code for the logarithm:
    % Gr = grassmannfactory(5, 2, 3);
    % x = Gr.rand()
    % y = Gr.rand()
    % u = Gr.log(x, y)
    % Gr.dist(x, y) % These two numbers should
    % Gr.norm(x, u) % be the same.
    % z = Gr.exp(x, u) % z needs not be the same matrix as y, but it should
    % v = Gr.log(x, z) % be the same point as y on Grassmann: dist almost 0.
    M.log = @logarithm;
    function U = logarithm(X, Y)
        U = zeros(n, p, k);
        for i = 1 : k
            x = X(:, :, i);
            y = Y(:, :, i);
            ytx = y.'*x;
            At = y.'-ytx*x.';
            Bt = ytx\At;
            [u, s, v] = svd(Bt.', 'econ');

            u = u(:, 1:p);
            s = diag(s);
            s = s(1:p);
            v = v(:, 1:p);

            U(:, :, i) = u*diag(atan(s))*v.';
        end
    end

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

    M.rand = @random;
    function X = random()
        X = zeros(n, p, k);
        for i = 1 : k
            [Q, unused] = qr(randn(n, p), 0); %#ok<NASGU>
            X(:, :, i) = Q;
        end
    end

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

    M.lincomb = @lincomb;

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

    % This transport is compatible with the polar retraction.
    M.transp = @(x1, x2, d) projection(x2, d);

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

end

% Linear combination of tangent vectors
function d = lincomb(x, a1, d1, a2, d2) %#ok<INUSL>

    if nargin == 3
        d = a1*d1;
    elseif nargin == 5
        d = a1*d1 + a2*d2;
    else
        error('Bad use of grassmann.lincomb.');
    end

end


function [ r, info ] = r8po_fa ( n, a )

%*****************************************************************************80
%
%% R8PO_FA factors a R8PO matrix.
%
%  Discussion:
%
%    The R8PO storage format is appropriate for a symmetric positive definite
%    matrix and its inverse.  (The Cholesky factor of a R8PO matrix is an
%    upper triangular matrix, so it will be in R8GE storage format.)
%
%    Only the diagonal and upper triangle of the square array are used.
%    This same storage scheme is used when the matrix is factored by
%    R8PO_FA, or inverted by R8PO_INVERSE.  For clarity, the lower triangle
%    is set to zero.
%
%    The positive definite symmetric matrix A has a Cholesky factorization
%    of the form:
%
%      A = R' * R
%
%    where R is an upper triangular matrix with positive elements on
%    its diagonal.  This routine overwrites the matrix A with its
%    factor R.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 February 2004
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Bunch, Moler, Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(N,N), the matrix in R8PO storage.
%
%    Output, real R(N,N), the Cholesky factor R in R8GE storage.
%
%    Output, integer INFO, error flag.
%    0, normal return.
%    K, error condition.  The principal minor of order K is not
%    positive definite, and the factorization was not completed.
%
  r(1:n,1:n) = a(1:n,1:n);

  for j = 1 : n

    for k = 1 : j - 1
      t = 0.0;
      for i = 1 : k-1
        t = t + r(i,k) * r(i,j);
      end
      r(k,j) = ( r(k,j) - t ) / r(k,k);
    end

    t = 0.0;
    for i = 1 : j - 1
      t = t + r(i,j)^2;
    end

    s = r(j,j) - t;

    if ( s <= 0.0 )
      info = j;
      return;
    end

    r(j,j) = sqrt ( s );

  end

  info = 0;
%
%  Since the Cholesky factor is stored in R8GE format, be sure to
%  zero out the lower triangle.
%
  for i = 1 : n
    for j = 1 : i-1
      r(i,j) = 0.0;
    end
  end

  return
end


function c = cond_2x2(A)
%COND   Condition number with respect to inversion.
%   COND2d(X) returns the 2-norm condition number (the ratio of the
%   largest singular value of X to the smallest).  Large condition
%   numbers indicate a nearly singular matrix.
%
%   COND(X,P) returns the condition number of X in 2-norm:
%
%      NORM(X,2) * NORM(INV(X),2).
%
%   Class support for input X:
%      float: double, single
%
%   See also RCOND, CONDEST, CONDEIG, NORM, NORMEST.

%   Copied from MATLAB's cond function.

assert(~issparse(A));
[d1,d2,n] = size(A);
assert(d1==2 && d2==2);
A = reshape(A,[4,n]);

% A2 = A'*A
tmp = A(1,:).*A(3,:) + A(2,:).*A(4,:);
A2 = cat(1,A(1,:).^2 + A(2,:).^2,...
  tmp,tmp,...
  A(3,:).^2 + A(4,:).^2);

s = sqrt(eigs_2x2(A2));
c = zeros([1,n],class(A));

issing = any(s==0,1);
c(issing) = inf;
c(~issing) = max(s(:,~issing),[],1)./min(s(:,~issing),[],1);


%
%   run an example of ordfilt3 on a noisy sphere. The example is the same as
%   that written by Olivier Salvado.

clear
randn('seed',0)

[x,y,z] = meshgrid(1:100,1:100,1:100);
sphere = 20 + 200*( ( sqrt((x-50).^2+(y-50).^2+(z-50).^2) ) < 40 );
clear x y z
sphere = uint8(1*sphere + 50*randn(size(sphere)));

% -- median filter
tic
[Vr] = ordfilt3(sphere,'med',5);
toc
clf
% -- compare to box filter
subplot(221)
p1 = patch(isosurface(sphere,100), ...
   'FaceColor','blue','EdgeColor','none');
p2 = patch(isocaps(sphere,100), ...
    'FaceColor','interp','EdgeColor','none');
isonormals(sphere,p1)
view(3); axis vis3d square
camlight; lighting phong

subplot(222)
p1 = patch(isosurface(Vr,100), ...
   'FaceColor','blue','EdgeColor','none');
p2 = patch(isocaps(Vr,100), ...
    'FaceColor','interp','EdgeColor','none');
isonormals(Vr,p1)
view(3); axis vis3d square
camlight; lighting phong

%%
% show some slice
for k=1:20,
    subplot(223)
    imagesc( sphere(:,:,k) ,[0 255]),axis image
    title('original')

    subplot(224)
    imagesc( Vr(:,:,k) ,[0 255]),axis image
    title('Filtered with a 3D median filter')

    drawnow
end


function yi = finterp1(x,y,xi,outOfRangeVal)
%"finterp1"
%   Fast 1-D linear interpolation of regularly spaced vector, to regularly
%   spaced vector/point.
%
% This function does the following
% transformation:	t = (xi-x(1))/(x(end)-x(1))
%                   yi = y(1) + t ( y(end) - y(1) )
%
%   x & y must be a monotonically increasing vector/array;
%
% Written DK 09-27-06
%
% Usage:
%       yi = finterp1(x,y,xi,outOfRangeVal);
%
% See also FINTERP2, FINTERP3, FINTERP3NOMESH ,INTERP1, INTERP2, INTERP3
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

if ~exist('outOfRangeVal')
    outOfRangeVal = NaN;
end

n = length(x);
% check if all the parameters passed are of the same class
superiorfloat(x,y,xi);

try
    h = diff(x);
    [ignore,k] = histc(xi,x);
    k(xi<x(1) | ~isfinite(xi)) = 1;
    k(xi>=x(n)) = n-1;
    t = (xi - x(k))./h(k);
catch % if the vector is monotonically decreasing
    t = (xi-x(1))/(x(end)-x(1));
    k = y(1) + t*(y(end) - y(1));
    k = round(k);
    k(xi<x(1) | ~isfinite(xi)) = 1;
    k(xi>=x(n)) = n-1;
end

yi = y(k)+t.*(y(k+1) - y(k));

% APA Check
% x = x(:);
% xi = xi(:);
% y = y(:);
% dxV = [diff(x); 1]; %DUMMY 1
% if all(dxV(1:end-1)<0)
%     x = flipud(x);
%     y = flipud(y);
%     dxV = [diff(x); 1];
% end
% dyV = [diff(y); 1]; %DUMMY 1
% indNaN = xi < min(x) | xi> max(x);
% xi(indNaN) = x(1); %assign DUMMY value
% [jnk,binIndex] = histc(xi,x);
% slopeV = dyV./dxV;
% slopeV(binIndex==length(x)) = 0;
% yi = y(binIndex) + slopeV(binIndex).*(xi-x(binIndex));
% yi(indNaN) = outOfRangeVal;
%
% return;


function [C,t,err] = cubic_vertex_removal(C1,C2,varargin)
  % CUBIC_VERTEX_REMOVAL Given a G¹ continuous sequence of cubic Bézier curves,
  % optimize the positions of a new single curve to minimize its integrated
  % squared distance to those curves. This requires both estimating the
  % parameterization mapping between the inputs and output (non-linear problem)
  % and then computing the optimal positions as a linear least squares problem.
  %
  % C = cubic_vertex_removal(C1,C2)
  % [C,t,err] = cubic_vertex_removal(C1,C2,'ParameterName',ParameterValue,…)
  %
  % Inputs:
  %   C1  4 by dim list of first curves control point positions
  %   C2  4 by dim list of second curves control point positions, it's assumed
  %     that C1(4,:) == C2(1,:) and C1(4,:) - C1(3,:) = s * (C2(2,:) - C2(1,:))
  %     for some s>= 0.
  %     Optional:
  %       'Method' followed by one of the following:
  %         {'perfect'}  use root finding. This is fastest when you only care about
  %           finding a perfect fit. It may lead to a very poor approximate when
  %           a perfect fit is not possible. A perfect fit is when
  %           `[C1,C2] = cubic_split(C,t)`. If this is the case, then the
  %           returned `err` for this method should be very close to zero. (Note
  %           that "perfect fit" → err=0 but err=0 does not necessarily imply
  %           "perfect fit".
  %         'iterative'  use iterative method. This is slower but more accurate
  %           when a perfect fit is not possible.
  %       'MaxIter'  followed by maximum number of iterations for iterative
  %         method {100}
  %       't0'  followed by initial guess of t for iterative method {relative
  %         approximate arc lengths}
  %       'AlreadyGenerated'  followed by whether the automatically generated helper
  %         functions cubic_vertex_removal_polyfun and cubic_vertex_removal_g
  %         have already been generated. On some machines checking `exist()` can
  %         be really slow. So, you could call
  %         `cubic_vertex_removal(…,'AlreadyGenerated',false)` onces to
  %         generate the files and the call
  %         `cubic_vertex_removal(…,'AlreadyGenerated',true) for subsequent
  %         calls {false}.
  % Outputs:
  %   C  4 by dim list of output coordinates. By default:
  %     C(1,:) = C1(1,:),
  %     C(4,:) = C2(4,:), (C₀ continuity)
  %       and
  %     C(2,:) - C(1,:) = s1*(C1(2,:) - C1(1,:)) with s1>=0
  %     C(4,:) - C(3,:) = s2*(C2(4,:) - C2(3,:)) with s2>=0 (G₁ continutity)
  %   t scalar value between [0,1] defining the piecewise-linear
  %     parameterization mapping between the inputs and output.  C1 is mapped to
  %     [0,t] and C2 is mapped to [t1,1].
  %   err  Integrated squared distance between the output and input curves (see
  %   cubic_cubic_integrated_distance).
  %
  % Example:
  %   C = [0 0;1 1;2 -1;3 0];
  %   tgt = 0.1;
  %   [C1,C2] = cubic_split(C,tgt);
  %   [C,t,err] = cubic_vertex_removal(C1,C2,'Method','perfect');
  %   clf;
  %   hold on;
  %   plot_cubic(C1,[],[],'Color',orange);
  %   plot_cubic(C2,[],[],'Color',orange);
  %   plot_cubic(C,[],[],'Color',blue);
  %   hold off;
  %   axis equal;
  %   set(gca,'YDir','reverse')
  %   title(sprintf('err: %g',err),'FontSize',30);
  %

  method = 'perfect';
  max_iter = 100;
  t0 = [];
  promise_already_built = false;
  E_tol = 1e-15;
  grad_tol = 1e-8;

  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Method','MaxIter','t0','AlreadyGenerated','Tol','GradientTol',}, ...
    {'method','max_iter','t0','promise_already_built','E_tol','grad_tol'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  if strcmp(method,'cubic-polish')
    [C,t,err] = cubic_vertex_removal(C1,C2,varargin{:},'Method','cubic');
    % Slip in 'MaxIter',1 before user parameters so it gets over-written if
    % provided.
    [C,t,err] = cubic_vertex_removal(C1,C2,'MaxIter',1,varargin{:},'t0',t,'Method','iterative');
    return;
  end

  switch method
  case {'perfect','cubic'}
    % _If_ t is perfect, then C is a simple function of t.
    C_from_t = @(C1,C2,t) ...
      [C1(1,:); ...
          (1./t)*(C1(2,:)-C1(1,:)) + C1(1,:); ...
      (1./(1-t))*(C2(end-1,:)-C2(end,:)) + C2(end,:); ...
      C2(end,:)];
    switch method
    case 'perfect'
      % Build a root finder for the 1D problem
      if ~promise_already_built && ~exist('cubic_vertex_removal_polyfun','file');
        warning('assuming L2 not l2 error');
        dim = 1;
        % Matlab is (sometimes?) confused that this is a static workspace and
        % refuses to let syms create variables.
        %syms('iC1',[4 dim],'real');
        %syms('iC2',[4 dim],'real');
        %% complains if I mark st as 'real'
        %syms('st',[1 1]);
        iC1 = [
          sym('iC11','real')
          sym('iC12','real')
          sym('iC13','real')
          sym('iC14','real')];
        iC2 = [
          sym('iC21','real')
          sym('iC22','real')
          sym('iC23','real')
          sym('iC24','real')];
        st = sym('st');

        sC = C_from_t(iC1,iC2,st);
        [oC1,oC2] = cubic_split(sC,st);
        % L2 style.
        sg = sum( [oC1-iC1;oC2-iC2].^2, 'all');
        sres = solve(diff(sg,st) == 0,st);
        %sdgdt = simplify(diff(sg,st));
        %vroots = @(C1,C2) double(vpasolve(subs(subs(sdgdt,iC1,C1),iC2,C2)==0,st,[0 1]));
        sres_children = children(sres(1));
        % Should cache these two:
        %polyfun = matlabFunction(flip(coeffs(sres_children(1),sres_children(2))),'Vars',{iC1,iC2});
        % Matlab2023a seems to use slightly different cell arrays for sres_children.
        polyfun = matlabFunction( ...
          flip(coeffs(sres_children{1},sres_children{2})), ...
          'Vars',{iC1,iC2}, ...
          'File','cubic_vertex_removal_polyfun');
        g = matlabFunction(sg,'Vars',{iC1,iC2,st},'File','cubic_vertex_removal_g');
      else
        polyfun = @cubic_vertex_removal_polyfun;
        g = @cubic_vertex_removal_g;
      end
      keepreal = @(C) C(imag(C)==0 & real(C)>=0 & real(C)<=1);
      % Using numerical roots is faster than vroots
      nroots = @(K1,K2) keepreal(roots(polyfun(K1,K2)));
      keepmin = @(ts,Es) ts(find(Es==min(Es),1));
      % We'll determine t based on the 1D g function. But we'll compute the
      % returned energy below using the full dim-D problem.
      keepmin = @(K1,K2,ts) keepmin(ts,arrayfun(@(t) g(K1,K2,t),ts));
      find_t = @(K1,K2) keepmin(K1,K2,nroots(K1,K2));
      % Decide which coordinate to use (pick a non-degenerate one).
      % Based on max-extent.
      [~,i] = max(max([C1(1,:);C2(end,:)])-min([C1(1,:);C2(end,:)]));
      t = find_t(C1(:,i),C2(:,i));
      assert(~isempty(t));
    case 'cubic'
      D1 = -6.*C1(1,:) + 18.*C1(2,:) - 18.*C1(3,:) + 6.*C1(4,:);
      D2 = -6.*C2(1,:) + 18.*C2(2,:) - 18.*C2(3,:) + 6.*C2(4,:);
      D2_sqr_len = sum(D2.*D2,2);
      D1_sqr_len = sum(D1.*D1,2);
      D2_D1 = sum(D2.*D1,2);
      r = D2_D1/D1_sqr_len;
      t = (1 + (r.^(1/3))).^-1;
    end
    C = C_from_t(C1,C2,t);
  case 'iterative'
    % use arc-length to guess t₁
    if isempty(t0)
      tol = 1e-5;
      ts = matrixnormalize(cumsum([0;spline_arc_lengths([C1;C2],[1 2 3 4;5 6 7 8],tol)]));
      t0 = ts(2);
    end
    t1 = t0;
    % Build null space matrices. so that C(:) = S*V + B(:) satisfies C₀ and G₁
    % constraints for any V
    B = [C1(1,:);C1(1,:);C2(4,:);C2(4,:)];
    B1 = [0 0;(C1(2,:) - C1(1,:));0 0;0 0];
    B2 = [0 0;0 0;(C2(3,:) - C2(4,:));0 0];
    S = [B1(:) B2(:)];

    f = @(t1) objective_t1(C1,C2,t1,B,S);
    [E,C] = f(t1);
    for iter = 1:max_iter
      %dfdt1 = (f(t1+1e-5)-f(t1-1e-5))/(2*1e-5);
      % Complex step is bit faster and more accurate/stable. For 1D input, I
      % believe this should be as good as autodiff and probably as good as we
      % can get without a lot of hand derivativation/compiling code.
      dfdt1 = imag(f(complex(t1,1e-100)))/1e-100;

      if norm(dfdt1,inf) < grad_tol
        break;
      end
      dt1 = 0.5*sign(-dfdt1);
      [alpha,t1] = backtracking_line_search(f,t1,dfdt1,dt1,0.3,0.5);
      if alpha == 0
        %warning('line search failed');
        break;
      end
      [E,C] = f(t1);
      if E < E_tol
        break;
      end
    end

    t = t1;
  otherwise
    error(['unknown method :' method]);
  end

  if nargout>2
    % L2 not l2
    [E1] = cubic_cubic_integrated_distance( ...
      0,t, ...
      1/t,0, ...
      C1, ...
      1,0, ...
      C);
    [E2] = cubic_cubic_integrated_distance( ...
      t,1, ...
      1/(1-t),-t/(1-t), ...
      C2, ...
      1,0, ...
      C);
    err = E1+E2;
  end

  % Helper functions for iterative method
  function [E,C] = objective_t1(C1,C2,t1,B,S)
    if isfloat(t1) && (t1>1 || t1<0)
      E = inf;
      return;
    end
    C = nan(4,2);
    [~,H,F,c] = objective(C1,C2,C,t1);
    % Enforce constraints via subspace
    % C = [
    %   C1(1,:)
    %   C1(1,:) + v1 * (C1(2,:) - C1(1,:))
    %   C2(4,:) + v2 * (C2(3,:) - C2(4,:))
    %   C2(4,:)
    %   ];
    HH = repdiag(H,2);
    V = ((S.'*HH*S)\(-S.'*F(:)-S.'*HH*B(:)));
    V = max(V,0);
    C = reshape( B(:) + S*V , size(C));
    [E] = objective(C1,C2,C,t1);
  end
  function [E,H,F,c] = objective(C1,C2,C,t1)
    if isfloat(t1) && (t1>1 || t1<0)
      E = inf;
      return;
    end
    % WARNING
    w1 = 1;
    w2 = 1;
    %w1 = t1;
    %w2 = 1-t1;
    if nargout == 4
      % Given t1 update C
      [H1,F1,c1,E1] = cubic_cubic_integrated_distance( ...
        0,t1, ...
        1/t1,0, ...
        C1, ...
        1,0, ...
        C);
      [H2,F2,c2,E2] = cubic_cubic_integrated_distance( ...
        t1,1, ...
        1/(1-t1),-t1/(1-t1), ...
        C2, ...
        1,0, ...
        C);
      % Equal weighting ("L2")
      H = w1*H1 + w2*H2;
      F = w1*F1 + w2*F2;
      c = w1*c1 + w2*c2;
    else
      % Given t1 update C
      [E1] = cubic_cubic_integrated_distance( ...
        0,t1, ...
        1/t1,0, ...
        C1, ...
        1,0, ...
        C);
      [E2] = cubic_cubic_integrated_distance( ...
        t1,1, ...
        1/(1-t1),-t1/(1-t1), ...
        C2, ...
        1,0, ...
        C);
    end
    E = w1*E1 + w2*E2;
  end
end


function pass = test_sign(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

% Initialise random vector:
seedRNG(6178);
x = 2 * rand(100, 1) - 1;
hvsde = @(x) .5*(sign(x) + 1);

%% Simple tests
pref.splitting = 0;
f = chebfun('x.^2', pref);
tol = eps*get(f, 'hscale');
f1 = sign(f);
pass(1,1) = all(feval(f1, x) == 1);

% Test if pointValues are dealt with correctly:
f = chebfun(@(x) cos(pi*x) + 2, sort(x)', pref);
f.pointValues = -pi*ones(length(f.ends), 1);
f1 = sign(f);
pass(1,2) = all(f1.pointValues == -1);

% Test also on longer intervals:
f = chebfun(@(x) x, [-1, 100], pref);
f1 = sign(f);
h = chebfun({-1, 1}, [-1, 0, 100]);
pass(1,3) = normest(h - f1) < tol;

% Test functions with breakpoints:
f = chebfun(@(x) x, [-1, 1e-10, 1], pref);
f1 = sign(f);
h = restrict(h,[-1, 1]);
pass(1,4) = normest(f1 - h) < tol;

% Real, imaginary and complex CHEBFUN objects:
f = chebfun({-1, 1, -1, 1, -1, 1}, -3:3);

%% Real CHEBFUN
gHandle1 = @(x) sin(pi*x);
g = chebfun(@(x) gHandle1(x), -3:3, pref);
g1 = sign(g);
pass(2,1) = length(g1.funs) == 6;
pass(2,2) = normest(f - g1) < tol;
pref.splitting = 1;
h1 = chebfun(@(x) sign(gHandle1(x)), -3:3, pref);
pass(2,3) = length(h1.funs) == 6;
pass(2,4) = normest(f - h1) < 100*tol;

%% Array-valued CHEBFUN
f1 = chebfun(@(x) feval(f, [x, x]) , -3:3, pref);
gHandle2 = @(x) cos(pi*(x-.5));
g = chebfun(@(x) [gHandle1(x), gHandle2(x)] , -3:3, pref);
g4 = sign(g);
pass(3,1) = length(g4.funs) == 6;
pass(3,2) = normest(f1 - g4) < 1e1*tol;

h4 = chebfun(@(x) sign([gHandle1(x), gHandle2(x)]), -3:3, pref);
pass(3,3) = length(h4.funs) == 6;
pass(3,4) = normest(f1 - h4) < tol;

%% A more complicated function:
f = chebfun(@(x) sin(1i*x).*(1i*x + exp(5i*x)));
g = chebfun(@(x) sign(sin(1i*x).*(1i*x + exp(5i*x))),[-1 0 1], ...
    'extrapolate', 'on');
h = sign(f);
pass(4,:) = normest(g - h) < 1e4*eps*length(h);

%% Test sign() for a complex-valued CHEBFUN.
f = chebfun(@(x) exp(2*pi*1i*x)./(1 + (x - 0.1).^2), [-1 1]);
h = sign(f);
h_exact = @(x) exp(2*pi*1i*x);
pass(5,:) = norm(feval(h, x) - h_exact(x), inf) < 1e2*vscale(h)*eps;

%% Test on singular function: a real case

% Set a domain:
dom = [-2 7];

% Generate a few random points to use as test values:
x = diff(dom) * rand(100, 1) + dom(1);

pow = -0.5;
op = @(x) (dom(2)-x).^pow.*( sin(10*x).^2 );
f = chebfun(op, dom, 'exps', [0 pow], 'splitting', 'on');
s = sign(f);
pass(6,:) = ( norm(feval(s-1, x), inf) < eps );

%% Test on singular function: a complex case

% Set a domain:
dom = [-1 1];

% Generate a few random points to use as test values:
x = diff(dom) * rand(100, 1) + dom(1);

pow = -0.5;
op = @(x) (dom(2)-x).^pow.*exp(2*pi*1i*x);
f = chebfun(op, dom, 'exps', [0 pow], 'splitting', 'on');
s = sign(f);
s_exact = @(x) exp(2*pi*1i*x);
vals_s = feval(s, x);
vals_exact = feval(s_exact, x);
err = vals_s - vals_exact;
pass(7,:) = ( norm(err, inf) < 1e1*eps*get(s, 'vscale') );

%% Functions on [-inf inf]:

% Set the domain:
dom = [-Inf Inf];
domCheck = [-1e2 1e2];

% Generate a few random points to use as test values:
x = diff(domCheck) * rand(100, 1) + domCheck(1);

op = @(x) (1-exp(-x.^2))./x;
f = chebfun(op, dom);
s = sign(f);
sVals = feval(s, x);
op = @(x) 2*hvsde(x) - 1;
sExact = op(x);
err = sVals - sExact;
pass(8,:) = all( ~norm(err, inf) );

end


clc, clear
x1=[6.683, 6.681, 6.676, 6.678, 6.679, 6.672];
x2=[6.661, 6.661, 6.667, 6.667, 6.664];
[h,p,ci,st]=ttest2(x1,x2,'Alpha',0.1)


function [ pivot, lu, info ] = r8mat_to_r8plu ( n, a )

%*****************************************************************************80
%
%% R8MAT_TO_R8PLU factors a general matrix.
%
%  Discussion:
%
%    This routine is a simplified version of the LINPACK routine DGEFA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, real A(N,N), the matrix to be factored.
%
%    Output, integer PIVOT(N), a vector of pivot indices.
%
%    Output, real LU(N,N), an upper triangular matrix U and
%    the multipliers L which were used to obtain it.  The factorization
%    can be written A = L * U, where L is a product of permutation and
%    unit lower triangular matrices and U is upper triangular.
%
%    Output, integer INFO, singularity flag.
%    0, no singularity detected.
%    nonzero, the factorization failed on the INFO-th step.
%
  lu(1:n,1:n) = a(1:n,1:n);

  info = 0;

  for k = 1 : n-1
%
%  Find L, the index of the pivot row.
%
    l = k;
    for i = k+1 : n
      if ( abs ( lu(l,k) ) < abs ( lu(i,k) ) )
        l = i;
      end
    end

    pivot(k) = l;
%
%  If the pivot index is zero, the algorithm has failed.
%
    if ( lu(l,k) == 0.0 )
      info = k;
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8MAT_TO_R8PLU - Fatal error!\n' );
      fprintf ( 1, '  Zero pivot on step %d\n', info );
      return
    end
%
%  Interchange rows L and K if necessary.
%
    if ( l ~= k )
      temp    = lu(l,k);
      lu(l,k) = lu(k,k);
      lu(k,k) = temp;
    end
%
%  Normalize the values that lie below the pivot entry A(K,K).
%
    lu(k+1:n,k) = -lu(k+1:n,k) / lu(k,k);
%
%  Row elimination with column indexing.
%
    for j = k+1 : n

      if ( l ~= k )
        temp    = lu(l,j);
        lu(l,j) = lu(k,j);
        lu(k,j) = temp;
      end

      lu(k+1:n,j) = lu(k+1:n,j) + lu(k+1:n,k) * lu(k,j);

    end

  end

  pivot(n) = n;

  if ( lu(n,n) == 0.0 )
    info = n;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_TO_R8PLU - Fatal error!\n' );
    fprintf ( 1, '  Zero pivot on step %d\n', info );
  end

  return
end


% Haar transform on Healpix
% Y = HealpixHaarTransform(X)

function Y = HealpixHaarTransform(X)

% nest depth
n = length(X);
m = log2(n / 12) / 2; % = log4(n / 12)

% Haar wavelet basis
A = [ 1  1  1  1;
      1  1 -1 -1;
      1 -1  1 -1;
      1 -1 -1  1 ] / sqrt(4);
%A = [ 1  1  1  1;
%      1  1 -1 -1;
%      sqrt(2) -sqrt(2)  0  0;
%      0  0  sqrt(2) -sqrt(2) ] / sqrt(4);

Y = HealpixNestedLinearTrans(X, A, m);


I = imread('bh.png');
%I = imread('C:\Users\Mostwanted\Desktop\Wuhan_China.jpg');
subplot(1,2,1);
imshow(I);

I=double(I);
f=I(:,:,1);
ff=I(:,:,2);
fff=I(:,:,3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k1=4;
k2=5;
r=161;
alf=1458;
nn=floor((r+1)/2);
for i=1:r
    for j=1:r
        b(i,j) =exp(-((i-nn)^2+(j-nn)^2)/(k1*alf))/(k2*pi*alf*10000); % Gaussian 1
   end
end

k1=8;
k2=8;
r=161;
alf=1458;
nn=floor((r+1)/2);
for i=1:r
    for j=1:r
        bb(i,j) =exp(-((i-nn)^2+(j-nn)^2)/(k1*alf))/(k2*pi*alf*10000);     % Gaussian 2
   end
end

k1=0.5;
k2=0.5;
r=161;
alf=1458;
nn=floor((r+1)/2);
for i=1:r
    for j=1:r
        bbb(i,j) =exp(-((i-nn)^2+(j-nn)^2)/(k1*alf))/(k2*pi*alf*10000);  % Gaussian 2 3
    end
    end
%%%%%%%%%%% R component of treatment %%%%%%%%%%%%%
Img = double(f);
[m,n]=size(f);

aa=125;

for i=1:m
    for j=1:n
        C(i,j)=log(1+aa*(Img(i,j)/I(i,j)));
    end
end

K=imfilter(Img,b);
KK=imfilter(Img,bb);
KKK=imfilter(Img,bbb);

for i=1:m
    for j=1:n
       G(i,j)=1/3*(log(Img(i,j)+1)-log(K(i,j)+1));
        G(i,j)=1/3*(log(Img(i,j)+1)-log(KK(i,j)+1))+G(i,j);
         G(i,j)=C(i,j)*(1/3*(log(Img(i,j)+1)-log(KKK(i,j)+1))+G(i,j));
    end
end

mi=min(min(G));
ma=max(max(G));
       L=(G-mi)*255/(ma-mi);
%%%%%%%%%%%%%% G Processing Components %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Img = double(ff);
[m,n]=size(ff);

aa=125;
for i=1:m
    for j=1:n
        CC(i,j)=log(1+aa*(Img(i,j)/I(i,j)));
    end
end

K=imfilter(Img,b);
KK=imfilter(Img,bb);
KKK=imfilter(Img,bbb);
for i=1:m
    for j=1:n
       G(i,j)=1/3*(log(Img(i,j)+1)-log(K(i,j)+1));
        G(i,j)=1/3*(log(Img(i,j)+1)-log(KK(i,j)+1))+G(i,j);
         G(i,j)=CC(i,j)*(1/3*(log(Img(i,j)+1)-log(KKK(i,j)+1))+G(i,j));
    end
end

mi=min(min(G));
ma=max(max(G));
       LL=(G-mi)*255/(ma-mi);
%%%%%%%%%%%%% With the B component of the Department %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Img = double(fff);
[m,n]=size(fff);

aa=125;
for i=1:m
    for j=1:n
        CCC(i,j)=log(1+aa*(Img(i,j)/I(i,j)));
    end
end

K=imfilter(Img,b);
KK=imfilter(Img,bb);
KKK=imfilter(Img,bbb);

for i=1:m
    for j=1:n
       G(i,j)=1/3*(log(Img(i,j)+1)-log(K(i,j)+1));
        G(i,j)=1/3*(log(Img(i,j)+1)-log(KK(i,j)+1))+G(i,j);
         G(i,j)=CCC(i,j)*(1/3*(log(Img(i,j)+1)-log(KKK(i,j)+1))+G(i,j));
    end
end

mi=min(min(G));
ma=max(max(G));

       LLL=(G-mi)*255/(ma-mi);
%%%%%%%%%%%% Department of Integrated color image of science %%%%%%%%%%%%%%%
msrcr=cat(3,L,LL,LLL);
subplot(1,2,2);
imshow(uint8(msrcr));
%imwrite(uint8(msrcr),'C:\Users\Mostwanted\Desktop\Wuhan_China_outcr1.jpg');
%imwrite(uint8(msrcr),'C:\Users\Mostwanted\Desktop\Washington_DC_outcr1.jpg');


function [Ri, RiFull] = syntheticCamera(F, type)

if nargin < 2
    type = 'continuous';
end

Ri = cell(F, 1);
RiFull = cell(F, 1);

switch type
    case 'random'
        for i = 1:F
            R = orth(randn(3));
            Ri{i} = R(2:3, :);
            RiFull{i} = R;
        end

    case 'continuous'
        Ux = @(u) [   0, -u(3),  u(2); ...
                   u(3),     0, -u(1); ...
                  -u(2),  u(1),    0];
        theta = randn(1); % create an angle randomly
        u = randn(3, 1);
        u = u/norm(u); % create a unit axis randomly
        for i = 1:F
            theta = theta + 1;
            % create a rotation matrix
            orthR = cos(theta)*eye(3) + sin(theta)*Ux(u) + (1-cos(theta))*kron(u,u');
            Ri{i} = orthR(2:3, :);
            RiFull{i} = orthR(1:3, :);
        end

    case 'vertical'
        Ux = @(u) [   0, -u(3),  u(2); ...
                   u(3),     0, -u(1); ...
                  -u(2),  u(1),    0];
        %theta = randn(1);
        theta = 360/F;
        u = [0.2;0;1]; % z-axis rotation
        for i = 1:F
            % in degrees
            orthR = cosd(theta*i)*eye(3) + sind(theta*i)*Ux(u) + (1-cosd(theta*i))*kron(u,u');

            Ri{i} = orthR(2:3, :);
            RiFull{i} = orthR(1:3, :);
        end

    case 'real'
        Ux = @(u) [   0, -u(3),  u(2); ...
                   u(3),     0, -u(1); ...
                  -u(2),  u(1),    0];
        %theta = randn(1);
        theta = 2;
        totalRotDeg = 120;
        u = [0;0;1]; % z-axis rotation
        for i = 1:F

            orthR = cosd(theta)*eye(3) + sind(theta)*Ux(u) + (1-cosd(theta))*kron(u,u');

            if i >= round(F/2) && i <= round((3*F)/4)
                camActNum = round(F/2) - round((3*F)/4);
                degrees = totalRotDeg / camActNum;

                % in degrees
                theta = theta + degrees;
                orthR = cosd(theta)*eye(3) + sind(theta)*Ux(u) + (1-cosd(theta))*kron(u,u');

                Ri{i} = orthR(2:3, :);
                RiFull{i} = orthR(1:3, :);
            else
                Ri{i} = orthR(2:3, :);
                RiFull{i} = orthR(1:3, :);
            end
        end

        case '360'
        Ux = @(u) [   0, -u(3),  u(2); ...
                   u(3),     0, -u(1); ...
                  -u(2),  u(1),    0];
        %theta = randn(1);
        theta = 360/F;
        u = [0;0;1]; % z-axis rotation
        for i = 1:F
            % in degrees
            orthR = cosd(theta*i)*eye(3) + sind(theta*i)*Ux(u) + (1-cosd(theta*i))*kron(u,u');
            Ri{i} = orthR(2:3, :);
            RiFull{i} = orthR(1:3, :);
        end

    otherwise
        error('%s is a wrong camera type!', type);
end


function degrees = degrees(radians)

% DEGREES (RADIANS)
%
% Conversion function from Radians to Degrees.
% Richard Medlock 12-03-2002
%
% Last Updated: 18-09-2009
% - Calculation simplified to make it more efficient
%   based on a suggestion by Joel Parker.

% Original calculation
% radians = radians/((2*pi)/360);

degrees = radians/(pi/180);


% Rank aware thresholding needs much more measurements than MUSIC to work
% properly.

close all;
clear all;
clc;
rng('default');
png_export = true;
pdf_export = false;
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');

mf = spx.graphics.Figures();

% Signal space
N = 256;
% Sparsity level
K = 6;
% Number of measurements
M = 32;
% Number of signals
S = 4;
% Construct the signal generator.
gen  = spx.data.synthetic.SparseSignalGenerator(N, K, S);
% Generate bi-uniform signals
X = gen.biUniform(1, 2);
% Sensing matrix
Phi = spx.dict.simple.gaussian_dict(M, N);
% Measurement vectors
Y = Phi.apply(X);
%  Rank Aware Thresholding solver instance
solver = spx.pursuit.joint.RankAwareThresholding(Phi, K);
% Solve the sparse recovery problem
result = solver.solve(Y);
% Solution vector
Z = result.Z;
% Comparison
cs = spx.commons.SparseSignalsComparison(X, Z, K);
cs.summarize();

for s=1:S
    mf.new_figure(sprintf('MMV signal: %d', s));
    subplot(411);
    stem(X(:, s), '.');
    title('Sparse vector');
    subplot(412);
    stem(Z(:, s), '.');
    title('Recovered sparse vector');
    subplot(413);
    stem(abs(X(:, s) - Z(:, s)), '.');
    title('Recovery error');
    subplot(414);
    stem(Y(:, s), '.');
    title('Measurement vector');
end


function pass = test_laplacian( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e7*pref.techPrefs.chebfuneps;

% Test with different parity of m,p
% Example 1:
f = ballfun(@(x,y,z)x.^2+y.^2+z.^2);
V = ballfunv(f,2*f,-f);
g = laplacian(V);
exact = ballfunv(ballfun(@(x,y,z)6),ballfun(@(x,y,z)12),ballfun(@(x,y,z)-6));
pass(1) = norm( g - exact ) < tol;

% Example 2:
f1 = ballfun(@(x,y,z)cos(x.*y));
f2 = ballfun(@(x,y,z)sin(y.*z));
f3 = ballfun(@(x,y,z)z.^3);
v = ballfunv(f1,f2,f3);
g = laplacian(v);
exact = grad(div(v))-curl(curl(v));
pass(2) = norm( g - exact ) < tol;

if (nargout > 0)
    pass = all(pass(:));
end
end


function [Z, Wsep, Wmix, m, U, S, V] = zca(X, portion)

if nargin < 2
    portion = 1;
end

rndidx = randperm(size(X, 1));
X_orig = X;
X = X(rndidx(1:round(size(X,1) * portion)), :);

m = mean(X, 1);
Xc = bsxfun(@minus, X, m);

sigma = Xc' * Xc / size(Xc, 1);

[U, S, V] = svd(sigma, 0);

Wsep = V / sqrt(S+1e-12);
Wmix = V';
Z = bsxfun(@minus, X_orig, m) * Wsep * Wmix;


function gdif = p00_gdif ( problem, n, x )

%*****************************************************************************80
%
%% P00_GDIF approximates the gradient via finite differences.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer PROBLEM, the problem number.
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the point where the gradient
%    is to be approximated.
%
%    Output, real GDIF(N), the approximated gradient vector.
%
  tol = eps^0.33;

  for i = 1 : n

    if ( 0.0 <= x(i) )
      dx = eps * ( x(i) + 1.0 );
    else
      dx = eps * ( x(i) - 1.0 );
    end

    xi = x(i);
    x(i) = xi + dx;
    fplus = p00_f ( problem, n, x );

    x(i) = xi - dx;
    fminus = p00_f ( problem, n, x );

    gdif(i) = ( fplus - fminus ) / ( 2.0 * dx );

    x(i) = xi;

  end

  return
end


function variance = cardioid_variance ( x, a, b )

%*****************************************************************************80
%
%% CARDIOID_VARIANCE returns the variance of the Cardioid PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 July 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 <= B <= 0.5.
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = a;

  return
end


function img = discreteSphereEighth(varargin)
%DISCRETESPHEREEIGHTH Discretize a 3D sphere eighth
%
%   IMG = discreteSphereEighth(LX, LY, LZ, SPHEIGHTH)
%   Creates a 3D image of a eighth of a sphere.
%
%   Example
%   img = discreteSphereEighth(1:100, 1:100, 1:100, [50 50 50 30 10 60 45]);
%   img = discreteSphereEighth([1 1 100;1 1 100;1 1 100], [50 50 50], 30, 10);
%   img = discreteSphereEighth([1 1 100;1 1 100;1 1 100], [50 50 50 30 10]);
%
%   See Also
%   imShapes, discreteBall, discreteCube
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2015-03-31
% Copyright 2015 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY

% compute coordinate of image voxels
[lx, ly, lz, varargin] = parseGridArgs3d(varargin{:});
[x, y, z] = meshgrid(lx, ly, lz);

% default parameters
center  = [lx(ceil(end/2)) ly(ceil(end/2)) lz(ceil(end/2))];
side    = center;
theta   = 0; phi = 0; psi=0;

% process input parameters
if length(varargin)==1
    var = varargin{1};
    center = var(:,1:3);
    if size(var, 2)>3
        side = var(:,4);
    end
    if size(var, 2)>4
        theta = var(:,5);
    end
    if size(var, 2)>5
        phi = var(:,6);
    end
    if size(var, 2)>6
        psi = var(:,7);
    end

elseif ~isempty(varargin)
    center = varargin{1};
    if length(varargin)>1
        side = varargin{2};
    end
    if length(varargin)>2
        theta = varargin{3};
    end
    if length(varargin)>3
        phi = varargin{4};
    end
    if length(varargin)>4
        psi = varargin{5};
    end
end

if length(side) == 1
    side = [side side side];
end

% compute coordinate of image voxels in cube reference system
trans = composeTransforms3d(...
    createTranslation3d(-center),...
    createRotationOz(-deg2rad(phi)),...
    createRotationOy(-deg2rad(theta)), ...
    createRotationOz(-deg2rad(psi)),...
    createScaling3d(1 ./ side));
[x, y, z] = transformPoint3d(x, y, z, trans);

% create image: simple threshold over 3 dimensions, and over radius
img = sqrt(x.^2 + y.^2 + z.^2) <= 1 & x >= 0 & x <= 1 & y >=0 & y <= 1 & z >= 0 & z <= 1;


function d = spline_pchip_set ( n, x, f )

%*****************************************************************************80
%
%% SPLINE_PCHIP_SET sets derivatives for a piecewise cubic Hermite interpolant.
%
%  Discussion:
%
%    This routine computes what would normally be called a Hermite
%    interpolant.  However, the user is only required to supply function
%    values, not derivative values as well.  This routine computes
%    "suitable" derivative values, so that the resulting Hermite interpolant
%    has desirable shape and monotonicity properties.
%
%    The interpolant will have an extremum at each point where
%    monotonicity switches direction.
%
%    The resulting piecewise cubic Hermite function may be evaluated
%    by SPLINE_PCHIP_VAL.
%
%    This routine was originally called "PCHIM".
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 August 2005
%
%  Author:
%
%    Original FORTRAN77 version by Fred Fritsch.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Fred Fritsch, Ralph Carlson,
%    Monotone Piecewise Cubic Interpolation,
%    SIAM Journal on Numerical Analysis,
%    Volume 17, Number 2, April 1980, pages 238-246.
%
%    Fred Fritsch, J Butland,
%    A Method for Constructing Local Monotone Piecewise Cubic Interpolants,
%    LLNL Preprint UCRL-87559, April 1982.
%
%  Parameters:
%
%    Input, integer N, the number of data points.  N must be at least 2.
%
%    Input, real X(N), the strictly increasing independent
%    variable values.
%
%    Input, real F(N), dependent variable values to be interpolated.  This
%    routine is designed for monotonic data, but it will work for any F-array.
%    It will force extrema at points where monotonicity switches direction.
%
%    Output, real D(N), the derivative values at the
%    data points.  If the data are monotonic, these values will determine
%    a monotone cubic Hermite function.
%

%
%  Check the arguments.
%
  if ( n < 2 )
    ierr = -1;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SPLINE_PCHIP_SET - Fatal error!\n' );
    fprintf ( 1, '  Number of data points less than 2.\n' );
    error ( 'SPLINE_PCHIP_SET - Fatal error!' );
  end

  for i = 2 : n
    if ( x(i) <= x(i-1) )
      ierr = -3;
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SPLINE_PCHIP_SET - Fatal error!\n' );
      fprintf ( 1, '  X array not strictly increasing.\n' );
      error ( 'SPLINE_PCHIP_SET - Fatal error!' );
    end
  end

  ierr = 0;
  nless1 = n - 1;
  h1 = x(2) - x(1);
  del1 = ( f(2) - f(1) ) / h1;
  dsave = del1;
%
%  Special case N=2, use linear interpolation.
%
  if ( n == 2 )
    d(1) = del1;
    d(n) = del1;
    return
  end
%
%  Normal case, 3 <= N.
%
  h2 = x(3) - x(2);
  del2 = ( f(3) - f(2) ) / h2;
%
%  Set D(1) via non-centered three point formula, adjusted to be
%  shape preserving.
%
  hsum = h1 + h2;
  w1 = ( h1 + hsum ) / hsum;
  w2 = -h1 / hsum;
  d(1) = w1 * del1 + w2 * del2;

  if ( pchst ( d(1), del1 ) <= 0.0 )

    d(1) = 0.0
%
%  Need do this check only if monotonicity switches.
%
  elseif ( pchst ( del1, del2 ) < 0.0 )

     dmax = 3.0 * del1;

     if ( abs ( dmax ) < abs ( d(1) ) )
       d(1) = dmax;
     end

  end
%
%  Loop through interior points.
%
  for i = 2 : nless1

    if ( 2 < i )
      h1 = h2;
      h2 = x(i+1) - x(i);
      hsum = h1 + h2;
      del1 = del2;
      del2 = ( f(i+1) - f(i) ) / h2;
    end
%
%  Set D(I)=0 unless data are strictly monotonic.
%
    d(i) = 0.0;

    temp = pchst ( del1, del2 );

    if ( temp < 0.0 )

      ierr = ierr + 1;
      dsave = del2;
%
%  Count number of changes in direction of monotonicity.
%
    elseif ( temp == 0.0 )

      if ( del2 ~= 0.0D+00 )
        if ( pchst ( dsave, del2 ) < 0.0 )
          ierr = ierr + 1;
        end
        dsave = del2;
      end
%
%  Use Brodlie modification of Butland formula.
%
    else

      hsumt3 = 3.0 * hsum;
      w1 = ( hsum + h1 ) / hsumt3;
      w2 = ( hsum + h2 ) / hsumt3;
      dmax = max ( abs ( del1 ), abs ( del2 ) );
      dmin = min ( abs ( del1 ), abs ( del2 ) );
      drat1 = del1 / dmax;
      drat2 = del2 / dmax;
      d(i) = dmin / ( w1 * drat1 + w2 * drat2 );

    end

  end
%
%  Set D(N) via non-centered three point formula, adjusted to be
%  shape preserving.
%
  w1 = -h2 / hsum;
  w2 = ( h2 + hsum ) / hsum;
  d(n) = w1 * del1 + w2 * del2;

  if ( pchst ( d(n), del2 ) <= 0.0 )
    d(n) = 0.0;
  elseif ( pchst ( del1, del2 ) < 0.0 )
%
%  Need do this check only if monotonicity switches.
%
    dmax = 3.0 * del2;

    if ( abs ( dmax ) < abs ( d(n) ) )
      d(n) = dmax;
    end

  end

  return
end


function n = poisson_fixed_time ( lambda, time )

%*****************************************************************************80
%
%% POISSON_FIXED_TIME counts the Poisson events in a fied time.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real LAMBDA, the average number of events per unit time.
%
%    Input, real TIME, the amount of time to observe.
%
%    Output, integer N, the number of Poisson events observed.
%
  n = 0;
  t = 0.0;

  while ( t < time )
    dt = - log ( rand ( 1, 1 ) ) / lambda;
    n = n + 1;
    t = t + dt;
  end

  return
end


function lambda = circulant2_eigenvalues ( n, x )

%*****************************************************************************80
%
%% CIRCULANT2_EIGENVALUES returns the eigenvalues of the CIRCULANT2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of rows and columns of A.
%
%    Output, complex LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  x = zeros ( n, 1 );
  for i = 1 : n
    x(i,1) = i;
  end

  w(1:n,1) = c8vec_unity ( n );

  lambda(1:n,1) =  x(n,1);
  for i = n-1 : -1 : 1
    lambda(1:n,1) = lambda(1:n,1) .* w(1:n,1) + x(i,1);
  end

  return
end


function triangle_ncc_rule_test05 ( )

%*****************************************************************************80
%
%% TEST05 demonstrates REFERENCE_TO_PHYSICAL_T3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2007
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;
  node_num = 3;

  node_xy = [ ...
    0.0, 0.0; ...
    1.0, 0.0; ...
    0.0, 1.0 ]';
  node_xy2 = [ ...
    1.0, 2.0; ...
    1.0, 1.0; ...
    3.0, 2.0 ]';
  point_show = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST05\n' );
  fprintf ( 1, '  REFERENCE_TO_PHYSICAL_T3 transforms a rule\n' );
  fprintf ( 1, '  on the unit (reference) triangle to a rule on\n' );
  fprintf ( 1, '  an arbitrary (physical) triangle.\n' );

  rule = 3;

  order_num = triangle_ncc_order_num ( rule );

  [ xy, w ] = triangle_ncc_rule ( rule, order_num );
%
%  Here is the reference triangle, and its rule.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The reference triangle:\n', rule );
  fprintf ( 1, '\n' );

  for node = 1 : 3
    fprintf ( 1, '  %8d  %14f  %14f\n', node, node_xy(1:2,node) );
  end

  area = triangle_area ( node_xy );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Rule %d for reference triangle\n', rule );
  fprintf ( 1, '  with area = %f\n', area );
  fprintf ( 1, '\n' );
  fprintf ( 1, '                X               Y               W\n' );
  fprintf ( 1, '\n' );

  for order = 1 : order_num
    fprintf ( 1, '  %8d  %14f  %14f  %14f\n', order, xy(1:2,order), w(order) );
  end
%
%  Transform the rule.
%
  xy2 = reference_to_physical_t3 ( node_xy2, order_num, xy );
%
%  Here is the physical triangle, and its transformed rule.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The physical triangle:\n' );
  fprintf ( 1, '\n' );

  for node = 1 : 3
    fprintf ( 1, '  %8d  %14f  %14f\n', node, node_xy2(1:2,node) );
  end

  area2 = triangle_area ( node_xy2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Rule %d for physical triangle', rule );
  fprintf ( 1, '  with area = %f\n', area2 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '                X               Y               W\n' );
  fprintf ( 1, '\n' );

  for order = 1 : order_num
    fprintf ( 1, '  %8d  %14f  %14f  %14f\n', order, xy2(1:2,order), w(order) );
  end

  return
end


function varargout = EarthGradient(f,varargin)
%GRADIENT Approximate gradient.
%   [FX,FY] = GRADIENT(F) returns the numerical gradient of the
%   matrix F. FX corresponds to dF/dx, the differences in x (horizontal)
%   direction. FY corresponds to dF/dy, the differences in y (vertical)
%   direction. The spacing between points in each direction is assumed to
%   be one. When F is a vector, DF = GRADIENT(F) is the 1-D gradient.
%
%   [FX,FY] = GRADIENT(F,H), where H is a scalar, uses H as the
%   spacing between points in each direction.
%
%   [FX,FY] = GRADIENT(F,HX,HY), when F is 2-D, uses the spacing
%   specified by HX and HY. HX and HY can either be scalars to specify
%   the spacing between coordinates or vectors to specify the
%   coordinates of the points.  If HX and HY are vectors, their length
%   must match the corresponding dimension of F.
%
%   [FX,FY,FZ] = GRADIENT(F), when F is a 3-D array, returns the
%   numerical gradient of F. FZ corresponds to dF/dz, the differences
%   in the z direction. GRADIENT(F,H), where H is a scalar,
%   uses H as the spacing between points in each direction.
%
%   [FX,FY,FZ] = GRADIENT(F,HX,HY,HZ) uses the spacing given by
%   HX, HY, HZ.
%
%   [FX,FY,FZ,...] = GRADIENT(F,...) extends similarly when F is N-D
%   and must be invoked with N outputs and either 2 or N+1 inputs.
%
%   Note: The first output FX is always the gradient along the 2nd
%   dimension of F, going across columns.  The second output FY is always
%   the gradient along the 1st dimension of F, going across rows.  For the
%   third output FZ and the outputs that follow, the Nth output is the
%   gradient along the Nth dimension of F.
%
%   Examples:
%       [x,y] = meshgrid(-2:.2:2, -2:.2:2);
%       z = x .* exp(-x.^2 - y.^2);
%       [px,py] = gradient(z,.2,.2);
%       contour(z), hold on, quiver(px,py), hold off
%
%   Class support for input F:
%      float: double, single
%
%   See also DIFF, DEL2.

%   Copyright 1984-2015 The MathWorks, Inc.

[f,ndim,loc,rflag] = parse_inputs(f,varargin);
nargoutchk(0,ndim);

% Loop over each dimension.

varargout = cell(1,ndim);
siz = size(f);
% first dimension
g  = zeros(size(f),class(f)); % case of singleton dimension
h = loc{1};
n = siz(1);
% Take forward differences on left and right edges
if n > 1
   g(1,:) = (f(2,:) - f(1,:))./h;
   g(n,:) = (f(n,:) - f(n-1,:))./h;
end

% Take centered differences on interior points
if n > 2
   h = 2*h;
   for nn = 2:n-1
       g(nn,:) = (f(nn+1,:)-f(nn-1,:)) ./ h;
   end
end

varargout{1} = g;

% second dimensions and beyond
if ndim == 2
    % special case 2-D matrices to support sparse matrices,
    % which lack support for N-D operations including reshape
    % and indexing
    n = siz(2);
    h = loc{2};
    g = zeros(size(f),class(f));

    % Take forward differences on left and right edges
    if n > 1
        g(:,1) = (f(:,2) - f(:,1))./h;
        g(:,n) = (f(:,n) - f(:,n-1))./h;
    end

    % Take centered differences on interior points
    if n > 2
        h = 2*h;
        g(:,2:n-1) = (f(:,3:n) - f(:,1:n-2)) ./ h;
    end
    varargout{2} = g;

elseif ndim > 2
    % N-D case
    for k = 2:ndim
        n = siz(k);
        newsiz = [prod(siz(1:k-1)) siz(k) prod(siz(k+1:end))];
        nf = reshape(f,newsiz);
        h = reshape(loc{k},1,[]);
        g  = zeros(size(nf),class(nf)); % case of singleton dimension

        % Take forward differences on left and right edges
        if n > 1
            g(:,1,:) = (nf(:,2,:) - nf(:,1,:))/(h(2)-h(1));
            g(:,n,:) = (nf(:,n,:) - nf(:,n-1,:))/(h(end)-h(end-1));
        end

        % Take centered differences on interior points
        if n > 2
            h = h(3:n) - h(1:n-2);
            g(:,2:n-1,:) = (nf(:,3:n,:) - nf(:,1:n-2,:)) ./ h;
        end

        varargout{k} = reshape(g,siz);
    end
end

% Swap 1 and 2 since x is the second dimension and y is the first.
if ndim > 1
    varargout(2:-1:1) = varargout(1:2);
elseif rflag
    varargout{1} = varargout{1}.';
end

%-------------------------------------------------------
function [f,ndim,loc,rflag] = parse_inputs(f,v)
%PARSE_INPUTS
%   [ERR,F,LOC,RFLAG] = PARSE_INPUTS(F,V) returns the spacing
%   LOC along the x,y,z,... directions and a row vector
%   flag RFLAG.

loc = {};

% Flag vector case and row vector case.
ndim = ndims(f);
vflag = false;
rflag = false;
if isvector(f)
    ndim = 1;
    vflag = true;
    if isrow(f) % Treat row vector as a column vector
        rflag = true;
        f = f.';
    end
end

indx = size(f);

% Default step sizes: hx = hy = hz = 1
if isempty(v)
    % gradient(f)
    loc = cell(1, ndims(f));
    for k = 1:ndims(f)
        loc(k) = {1:indx(k)};
    end
elseif isscalar(v) % gradient(f,h)
    % Expand scalar step size
    if isscalar(v{1})
        loc = cell(1, ndims(f));
        for k = 1:ndims(f)
            h = v{1};
            loc(k) = {h*(1:indx(k))};
        end
        % Check for vector case
    elseif vflag
        loc(1) = v(1);
    else
        error(message('MATLAB:gradient:InvalidInputs'));
    end
elseif ndims(f) == numel(v)  % gradient(f,hx,hy,hz,...)
    % Swap 1 and 2 since x is the second dimension and y is the first.
    loc = v;
    if ndim > 1
        loc(2:-1:1) = loc(1:2);
    end
else
    error(message('MATLAB:gradient:InvalidInputs'));
end


function [H_x] = H_xfn(kMinus1State, imuMeasurement, deltaT )
%H_XFN Compute the 6x6 matrix H_x_k

psiVec = imuMeasurement.omega*deltaT;
psiMag = norm(psiVec);
d = imuMeasurement.v*deltaT;

Psi = cos(psiMag)*eye(3) + (1 - cos(psiMag))*(psiVec/psiMag)*(psiVec/psiMag)' - sin(psiMag)*crossMat(psiVec/psiMag);

%Construct the H_x matrix
H_x = zeros(6,6);
H_x(1:3, 1:3) = eye(3);
H_x(4:6, 4:6) = Psi;
H_x(1:3, 4:6) = -kMinus1State.C_vi'*crossMat(d);

end


clear; close all;

d = 100;
beta = 1e-1;
X = rand(1,d);
w = randn;
b = randn;
t = w'*X+b+beta*randn(1,d);
x = linspace(min(X),max(X),d);   % test data

[model,llh] = linRegVb(X,t);
% [model,llh] = rvmRegVb(X,t);
plot(llh);
[y, sigma] = linRegPred(model,x,t);
figure
plotCurveBar(x,y,sigma);
hold on;
plot(X,t,'o');
hold off


function orientation  = FlyOrient(subset_frame, threshold)

%FLYORIENT
%   Usage:
%       orientation = FlyOrient(subset_frame, threshold)
%
% This function takes in a subset frame around the fly (calculated by
% FindFly) and discards the 3D data by placing points where the pixel intensity
% is larger than the user chosen threshold.  Then, Principal Components
% Analysis (by way fot the pca1 function) is performed on the resulting
% scatter plot to find the direction of maximum variance --- this direction
% is taken to be the fly's (ambiguous) orientation.
% orientation is a vector consisting of two angles (complements) that comprise
% the body axis. The first element is an angle in the upper half plane; the
% second element is an angle in the lower half plane.

% Written by Dan Valente
% 11 October 2006

%Normalize frame data by pixel of maximum intensity
subset_frame = subset_frame/max(max(subset_frame));

% Put dots where fly is and do PCA on reduced data set
[rows, cols] = find(subset_frame >= threshold);
rows = length(subset_frame(:,1))-rows+1;
x = [cols';rows'];
[xnew, PC, V, data] = pca1(x);

% Find orientation vectors (two, mirrored across diagonal), and group into
% upper half and lower half planes.
a1 = PC(1,1);
b1 = PC(2,1);
a2 = -PC(1,1);
b2 = -PC(2,1);
if (b1 >= 0 );
    orientUHP = atan2(b1,a1);
    orientLHP = atan2(b2,a2);
elseif (b2 >=0);
    orientUHP = atan2(b2,a2);
    orientLHP = atan2(b1,a1);
else
end

% The vector we will return
orientation = [orientUHP orientLHP];

return;


function polygon_properties_test02 ( )

%*****************************************************************************80
%
%% POLYGON_PROPERTIES_TEST02 tests POLYGON_AREA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2014
%
%  Author:
%
%    John Burkardt
%
  test_num = 2;
  area_exact_test = [ 2.0, 6.0 ];
  n_test = [ 4, 8 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLYGON_PROPERTIES_TEST02\n' );
  fprintf ( 1, '  For a polygon:\n' );
  fprintf ( 1, '  POLYGON_AREA computes the area.\n' );

  for test = 1 : test_num

    n = n_test(test);
    area_exact = area_exact_test(test);

    if ( test == 1 )

      v = [ ...
        1.0, 0.0; ...
        2.0, 1.0; ...
        1.0, 2.0; ...
        0.0, 1.0 ]';

    elseif ( test == 2 )

      v = [ ...
        0.0, 0.0; ...
        3.0, 0.0; ...
        3.0, 3.0; ...
        2.0, 3.0; ...
        2.0, 1.0; ...
        1.0, 1.0; ...
        1.0, 2.0; ...
        0.0, 2.0 ]';

    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Number of polygonal vertices = %d\n', n );

    r8mat_transpose_print ( 2, n, v, '  The polygon vertices:' );

    area = polygon_area ( n, v );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Exact area is        %g\n', area_exact );
    fprintf ( 1, '  The computed area is %g\n', area );

  end

  return
end


%
% Written by M. Harper Langston - 5/10/00
% harper@cims.nyu.edu
%
% For this sparse LU solver of Ax = b, A has band s.
% For example, the following matrix has band s = 1
%
%   x x 0 0 0 0
%   x x x 0 0 0
%   0 x x x 0 0
%   0 0 x x x 0
%   0 0 0 x x x
%   0 0 0 0 x x
%
%
function [x] = Band_Solve(A,b)
% Here, s is the band number
[m,n] = size(A);
if m~=n
   error('Matrix not sqaure') % Want square matrix for simplicity
end
% Find the band of A
for l = 1:n
   if A(m,l)~=0
      s = n-l;
      break;
   end
end
% Do the sparse LU decomposition for a matrix of band s
for j = 1:m
   if j >= m-s
      L(j:m,j) = A(j:m,j)./A(j,j);
		U(j,j:m) = A(j,j:m);
   	A(j:m,j:m) = A(j:m,j:m) - L(j:m,j)*U(j,j:m);
   else
   	L(j:j+s,j) = A(j:j+s,j)./A(j,j);
   	U(j,j:j+s) = A(j,j:j+s);
   	A(j:j+s,j:j+s) = A(j:j+s,j:j+s) - L(j:j+s,j)*U(j,j:j+s);
   end
end
L = sparse(L);
U = sparse(U);
% Call backsolve routine, which I wrote to pay heed to sparsity.
[y] = Back_Solve(L,b);
[x] = Back_Solve(U,y);
%
% Written by M. Harper Langston - 5/10/00
%


function c=ref_rdftiii_1(f)
%REF_RDFTIII_1  Reference RDFT by FFT
%   Usage:  c=ref_rdftiii_1(f);
%
%   Compute RDFTII by doing a DFTIII and returning half the coefficients.
%   Only works for real functions.
%
%   The transform is orthonormal

L=size(f,1);
Lhalf=floor(L/2);
Lend=Lhalf*2;

cc=ref_dftiii(f);

c=zeros(size(f));

% Copy the cosine-part of the coefficients.
c(1:2:Lend,:)=sqrt(2)*real(cc(1:Lhalf,:));

% Copy the sine-part of the coefficients.
c(2:2:Lend,:)=-sqrt(2)*imag(cc(1:Lhalf,:));

% If f has an odd length, we must also copy the Niquest-wave
% (it is real)
if mod(L,2)==1
  c(end,:)=real(cc((L+1)/2,:));
end;


%TR2RPY	Convert a homogeneous transform matrix to roll/pitch/yaw angles
%
%	[A B C] = TR2RPY(TR) returns a vector of Euler angles
%	corresponding to the rotational part of the homogeneous transform TR.
%
%	See also  RPY2TR, TR2EUL

%	Copright (C) Peter Corke 1993
function rpy = tr2rpy(m)

	rpy = zeros(1,3);

	if abs(m(1,1)) < eps & abs(m(2,1)) < eps,
		rpy(1) = 0;
		rpy(2) = atan2(-m(3,1), m(1,1));
		rpy(3) = atan2(-m(2,3), m(2,2));
	else,
		rpy(1) = atan2(m(2,1), m(1,1));
		sp = sin(rpy(1));
		cp = cos(rpy(1));
		rpy(2) = atan2(-m(3,1), cp * m(1,1) + sp * m(2,1));
		rpy(3) = atan2(sp * m(1,3) - cp * m(2,3), cp*m(2,2) - sp*m(1,2));
	end


function btv_test04 ( )

%*****************************************************************************80
%
%% BTV_TEST04 tests BURGERS_TIME_VISCOUS with the shock initial condition.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BTV_TEST04\n' );
  fprintf ( 1, '  Test BURGERS_TIME_VISCOUS with the shock initial condition.\n' );
  fprintf ( 1, '  Use periodic boundaries.\n' );

  nx = 81;
  nt = 300;
  t_max = 3.0;
  nu = 0.01;
  bc = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial condition: shock\n' );
  fprintf ( 1, '  Number of space nodes = %d\n', nx );
  fprintf ( 1, '  Number of time steps = %d\n', nt );
  fprintf ( 1, '  Final time T_MAX = %g\n', t_max );
  fprintf ( 1, '  Viscosity = %g\n', nu );
  fprintf ( 1, '  Boundary condition = %d\n', bc );

  U = burgers_time_viscous ( @ic_shock, nx, nt, t_max, nu, bc );

  x = linspace ( -1.0, +1.0, nx );

  figure ( 4 )

  plot ( x, U(1:50:(nt+1),:), 'Linewidth', 3 )
  grid on
  xlabel ( '<-- X -->' )
  ylabel ( '<-- U(X,T) -->' )
  title ( 'Burgers equation solutions over time, initial condition shock' )

  filename = 'btv_test04.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plot as "%s"\n', filename );

  return
end


% polyvalm2 - Evaluate polynomial with matrix argument.
%*************************************************************************************
%
%  MATLAB (R) is a trademark of The Mathworks (R) Corporation
%
%  Function:    polyvalm2
%  Filename:    polyvalm2.m
%  Programmer:  James Tursa
%  Version:     1.1
%  Date:        November 11, 2009
%  Copyright:   (c) 2009 by James Tursa, All Rights Reserved
%
%  This code uses the BSD License:
%
%  Redistribution and use in source and binary forms, with or without
%  modification, are permitted provided that the following conditions are
%  met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
%  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
%  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
%  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
%  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
%  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
%  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
%  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
%  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
%  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
%  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
%  POSSIBILITY OF SUCH DAMAGE.
%
%  polyvalm2 evaluates a polynomial with a square matrix argument faster
%  than the MATLAB built-in functions polyvalm or mpower.
%
%   Y = polyvalm2(P,X), when P is a vector of length N+1 whose
%   elements are the coefficients of a polynomial, is the value
%   of the polynomial evaluated with matrix argument X.  X must
%   be a square matrix.
%
%       Y = P(1)*X^N + P(2)*X^(N-1) + ... + P(N)*X + P(N+1)*I
%
%   Class support for inputs P, X:
%      float: double, single
%
%  The polyvalm2 speed improvements come from the following:
%
%  1) The MATLAB built-in function polyvalm uses Horner's method.
%     polyvalm2 uses a binary decomposition of the matrix powers
%     to do the calculation more efficiently, reducing the total
%     number of matrix multiplies used to calculate the answer.
%
%  2) polyvalm calculates the product of a scalar times a matrix
%     as the product of diag(scalar*ones(etc))*matrix ... i.e. it
%     does a matrix multiply. polyvalm2 will calculate this more
%     efficiently as the simple product scalar*matrix.
%
%  3) polyvalm does all of the calculations shown above, even if
%     the coefficient P(i) is zero. polyvalm2 does not do
%     calculations for P(i) coefficients that are zero.
%
%  4) polyvalm converts sparse matrix inputs into full matrices to
%     do the calculations, whereas polyvalm2 keeps the intermediate
%     calculations and the answer sparse.
%
%  An extreme case of speed difference can be found with a sparse
%  matrix example:
%
%  >> A = sprand(2500,2500,.01);
%  >> p = [1 2 3 4];
%  >> tic;polyvalm(p,A);toc
%     Elapsed time is 43.669362 seconds.
%  >> tic;polyvalm2(p,A);toc
%     Elapsed time is 4.240375 seconds.
%
%  The trade-off is that polyvalm2 uses more memory for intermediate
%  variables than polyvalm, so for very large matrices polyvalm2 can
%  run out of memory. In these cases polyvalm2 will abandon the
%  efficient calculation method and just call the built-in polyvalm.
%  For sparse matrix inputs, however, polyvalm2 will typically be more
%  memory efficient than the MATLAB polyvalm function.
%
%  Caution: Since polyvalm2 uses different calculations to form the matrix
%  powers, the end result may not match polyvalm exactly. Also, for the
%  case where only the leading coefficient is non-zero, polyvalm2 may not
%  match mpower exactly. But the answer will be just as accurate. And if
%  there are inf's or NaN's involved, then the end result will not, in
%  general, match polyvalm or mpower results. This should not be a great
%  drawback to using polyvalm2, however, since even the MATLAB built-in
%  functions polyvalm and mpower will not match each other in these cases.
%  (By reordering the calculations, the NaN's propagate differently)
%
%  Change Log:
%  Nov 11, 2009: Updated for sparse matrix input --> sparse result
%
%**************************************************************************

function Y = polyvalm2(p,X)
%\
% Check the arguments
%/
if( nargin ~= 2 )
    error('MATLAB:polyvalm2:InvalidNumberOfArgs','Need two input arguments.');
end
if( nargout > 1 )
    error('MATLAB:polyvalm2:TooManyOutputArgs','Too many output arguments.');
end
classname = superiorfloat(p,X);
if( ~(isvector(p) || isempty(p)) )
    error('MATLAB:polyvalm2:InvalidP','P must be a vector.');
end
z = size(X);
if( length(z) > 2 || z(1) ~= z(2) )
    error('MATLAB:polyvalm2:NonSquareMatrix','Matrix must be square.');
end
if( isempty(X) )
    if( issparse(X) )
        Y = sparse(z(1),z(2));
    else
        Y = zeros(z,classname);
    end
    return
end
%\
% Clear out any leading zeros and reverse the coefficients
%/
try
    f = find(p,1,'first');
    if( isempty(f) )
        if( issparse(X) )
            Y = sparse(z(1),z(2));
        else
            Y = zeros(z,classname);
        end
        return
    end
    p2 = p(end:-1:f);
%\
% Initialize return value with the constant term, and then set the
% constant term coefficient to 0 so we don't process it anymore.
%/
    if( issparse(X) )
        Y = diag(p2(1) * sparse(ones(z(1),1)));
    else
        Y = diag(p2(1) * ones(z(1),1,classname));
    end
    p2(1) = 0;
%\
% Special small cases use custom code for quick return
%/
    np = length(p2);
    if( np == 1 )
        return
    elseif( np == 2 )
        if( p2(2) == 1 )
            Y = Y + X;
        else
            Y = Y + p2(2) * X;
        end
        return
    end
%\
% Initialize the cell array that will hold the X powers
%/
    cp{np} = [];
%\
% Get the binary decomposition of the powers as rows of binary characters,
% each row representing a different power, and arranged from 0 at the top
% to the highest power at the bottom.
%/
    bp = dec2bin(0:(np-1));
%\
% Loop through the bit positions from least significant to most significant
%/
    P = X;
    zz = size(bp,2);
    for n=zz:-1:1
%\
% Only process those rows with non-zero coefficients. If the bit position
% for this row is 1, then we need to apply the current power of X to the
% cell for this row. Each cell is building up the appropriate power of X.
% cp{1} is for X^0 (not used or needed actually because we have that piece
% already calculated from above), cp{2} is for X^1, cp{3} is for X^2, etc.
% P is the current power of X, i.e. P = X^(2^(zz-n))
%/
        check = (p2 ~= 0);
        for m=1:np
            if( check(m) && bp(m,n) == '1' )
                if( isempty(cp{m}) )
                    cp{m} = P;
                else
                    cp{m} = cp{m} * P;
                end
%\
% Look at all the downstream bit patterns. If any match the current one for
% the bits processed so far, then just copy the current cell into the
% downstream cells (no need to repeat the calculation downstream because we
% already know the answer). Then reset the check flag for that downstream
% row so we don't process it for this particular loop index.
%/
                for k=m+1:np
                    if( check(k) && isequal(bp(m,n:zz),bp(k,n:zz)) )
                        cp{k} = cp{m};
                        check(k) = 0;
                    end
                end
%\
% If the remaining leftmost bits of the current row are 0, then we are done
% with this power of X. Apply the coefficient and add it to the result,
% then free up the memory for this cell position. Also, reset the
% coefficient for this row to 0 so we know not to process this row anymore.
%/
                if( all(bp(m,1:(n-1))=='0') )
                    Y = Y + p2(m) * cp{m};
                    p2(m) = 0;
                    cp{m} = [];
                end
            end
        end
%\
% Square the current power of X, but not if it is the last index in the
% loop because in that case it won't be used or needed. For some reason,
% P * P is a lot faster than P^2.
%/
        if( n ~= 1 )
            P = P * P;
        end
    end
%\
% The binary decomposition scheme used too much memory, so clear all of the
% local large variables and just call the built in polyvalm function
% instead. This is slower and computationally less efficient, but it is
% more memory efficient so it might work.
%/
catch
    warning('MATLAB:polyvalm2:OutOfMemory','Out Of Memory ... Resorting to built-in polyvalm');
    clear cp P bp p2 check;
    Y = polyvalm(p,X);
end
return
end


function M = mult(A, f, lambda)
%MULT   Multiplication operator for the ultraspherical spectral method.
%   M = MULT(A, F, lambda) returns the multiplication operator that represents
%   u(x) -> F(x)u(x), in the C^{(lambda)} ultraspherical polynomial basis.
%
%   If lambda = 0, then the operator is Toeplitz-plus-Hankel-plus-rank-1 and
%   represents multiplication in Chebyshev T coefficients.
%
%   If lambda = 1, then the operator is Toeplitz-plus-Hankel and represents
%   multiplication in Chebyshev U or C^{(1)} coefficients.
%
%   If lambda > 1, then the operator does not have any Toeplitz/Hankel structure
%   and is constructed using a three-term recurrence.

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

% Obtaining some useful information:
n = A.dimension;
d = A.domain;
f = restrict(f, d);
numIntervals = length(d) - 1;

% Find the diagonal blocks;
blocks = cell(numIntervals);
for k = 1:numIntervals
    blocks{k} = ultraS.multmat(n(k), f.funs{k}, lambda);
end

% Assemble:
M = blkdiag(blocks{:});

end


%% Band-Pass filter visualization

[b, a] = butter(4, [0.5 50] / 100, 'bandpass');
y = filter(b, a, cnt.x);
cnt.x=cnt.x(:,1:34);
cnt.clab=cnt.clab(:,1:34);

f_sz=ceil(length(cnt.x)/2);
f=100*linspace(0,1,f_sz);
f_X=fft(cnt.x);
f_y=fft(y);
subplot(2,1,1)
stem(f,abs(f_X(1:f_sz)));
title('Original signal');
xlabel('frequency');
xlim([0 50]);
ylabel('power');
subplot(2,1,2)
stem(f,abs(f_y(1:f_sz)));
xlim([0 50]);

title('Application of the beta (13-30Hz) bandpass filter')
xlabel('frequency');
ylabel('power');


function [sigma,u,eqn,info] = elasticitymfemP1P0(node,elem,pde,bdFlag,option)

if ~exist('option','var'), option = []; end

time = cputime;  % record assembling time

d = 2; % two dimensions
N = size(node,1);
NT = size(elem,1);
Nsigma = 3*N;
Nu = 2*NT;
Ndof = Nsigma + Nu;

%% Assemble matrix
[Dphi,area] = gradbasis(node,elem);

%% Mass matrix of linear (P1) element
M = sparse(N,N);
for i = 1:3
    for j = i:3
        ii = double(elem(:,i));
        jj = double(elem(:,j));
        if (j==i)
            M = M + sparse(ii,jj,area/6,N,N);
        else
            M = M + sparse([ii;jj],[jj;ii],[area/12; area/12],N,N);
        end
    end
end

%% Compliance tensor
lambda = pde.lambda;
mu = pde.mu;
C1 = 1/(2*mu);
C2 = lambda/(2*mu + d*lambda);
% E = mu*(3*lambda+2*mu)/(lambda+mu);
% nu = lambda/(2*(lambda+mu));
A = sparse(C1*(eye(3,3) - C2*([1 1 0]'*[1 1 0])));

%% Matrix for (Asigma,tau)
Am = kron(A,M);

%% Div operator
elem2dofsigma = zeros(NT,3,3);
elem2dofsigma(:,:,1) = double(elem);
for k = 2:3
    elem2dofsigma(:,:,k) = elem2dofsigma(:,:,k-1)+N;
end
elemIdx = (1:NT)';
elem2dofu = [elemIdx elemIdx+NT];
Dx = squeeze(Dphi(:,1,:)).*repmat(area,1,3);
Dy = squeeze(Dphi(:,2,:)).*repmat(area,1,3);
clear Dphi
% u1: dx sigma(1) + dy sigma(3)
% u2: dx sigma(3) + dy sigma(2)
B = sparse(repmat(elem2dofu(:,1),1,3),elem2dofsigma(:,:,1),Dx,Nu,Nsigma) ...
  + sparse(repmat(elem2dofu(:,1),1,3),elem2dofsigma(:,:,3),Dy,Nu,Nsigma) ...
  + sparse(repmat(elem2dofu(:,2),1,3),elem2dofsigma(:,:,3),Dx,Nu,Nsigma) ...
  + sparse(repmat(elem2dofu(:,2),1,3),elem2dofsigma(:,:,2),Dy,Nu,Nsigma);

%% Stabilization
T = auxstructure(elem);
edge2elem = T.edge2elem;
elem2edge = T.elem2edge;
[normal,edgeLength,unitNormal] = edgenormal(node,T.edge);
clear T;
harea = edgeLength.^2;
% elementwise part: [u_in_k']:[u_jn_k']=n_k(i)n_k(j);
% Use Dphi as a scaled outwards normal vector to compute - int_F h[u n'][v n']
C = sparse(Nu,Nu);
for i = 1:2
    for j = i:2
        ii = elem2dofu(:,i);
        jj = elem2dofu(:,j);
        Cij = zeros(NT,1);
        for k = 1:3 % sum of all four faces
            fk = elem2edge(:,k);
            Cij = Cij + harea(fk).*((i==j) + unitNormal(fk,i).*unitNormal(fk,j));
        end
        if (j==i)
            C = C + sparse(ii,jj,-Cij,Nu,Nu);
        else
            C = C + sparse([ii;jj],[jj;ii],repmat(-Cij,1,2),Nu,Nu);
        end
    end
end
% cross face part
for i = 1:2
    for j = 1:2
        ii = elem2dofu(edge2elem(:,1),i);
        jj = elem2dofu(edge2elem(:,2),j);
        Cij = harea.*((i==j) + unitNormal(:,i).*unitNormal(:,j));
        C = C + sparse([ii;jj],[jj;ii],repmat(-Cij,1,2),Nu,Nu);
    end
end

%% Assemble the right hand side
F = zeros(Ndof,1);
fu = zeros(NT,2);
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isfield(option,'fquadorder')
    option.fquadorder = 2;   % default order
end
if ~isempty(pde.f)
	[lambda,weight] = quadpts3(option.fquadorder);
	nQuad = size(lambda,1);
    for p = 1:nQuad
		% quadrature points in the x-y coordinate
		pxy = lambda(p,1)*node(elem(:,1),:) ...
			+ lambda(p,2)*node(elem(:,2),:) ...
			+ lambda(p,3)*node(elem(:,3),:);
		fp = pde.f(pxy);
		fu = fu - weight(p)*fp;
    end
    fu = fu.*repmat(area,1,2);
end
clear fp
F((Nsigma+1):Ndof,1) = fu(:);

%% Boundary Conditions
if ~exist('bdFlag','var'), bdFlag = []; end
eqn = struct('Am',Am,'B',B,'C',C,'f',F(1:Nsigma),'g',F(Nsigma+1:end));
assembleTime = cputime - time;
%% Solver
bigA = [Am B'; B C];
bigu = bigA\F;
sigma = bigu(1:Nsigma);
u = bigu(Nsigma+1:end);

%% Output information
info.assembleTime = assembleTime;


function [db,f]=lpccc2db(cc,np,nc,c0)
%LPCCC2DB Convert complex cepstrum to dB power spectrum DB=(CC,NP,NC)
%
%  Inputs: cc(nf,n)     Complex ceptral coefficients excluding c(0), one frame per row
%          np           Size of output spectrum is np+1 [n]
%                       Alternatively, np can be a vector of output frequencies in the range 0 to 0.5
%          nc           Highest cepstral coefficient to use [np or, if np is a vector, n]
%                       Set nc=-1 to use n coefficients
%          c0(nf,1)     Cepstral coefficient cc(0) [0]
%
% Outputs: db(nf,np+2)  Power spectrum from DC to Nyquist in dB
%          f(1,np+2)    Normalized frequencies (0 to 0.5)
%
% The "complex cepstral coefficients", cc(n), are the inverse discrete-time Fourier transform
% of the log of the complex-valued spectrum. The cc(n) are real-valued and, for n<0, cc(n)=0.
% The "real cepstral coeffcients", rc(n), are the inverse discrete-time Fourier transform
% of the log of the magnitude spectrum; rc(0)=cc(0) and rc(n)=0.5*cc(n) for n~=0.
% For highest speed, choose np to be a power of 2.

%      Copyright (C) Mike Brookes 1998-2014
%      Version: $Id: lpccc2db.m 5025 2014-08-22 17:07:24Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nf,mc]=size(cc);
if nargin<2 || ~numel(np)
    if nargout
        np=mc;
    else
        np=128;
    end
end
k=10/log(10);
if nargin>=3 && numel(nc)==1 && nc==-1 nc=mc; end
if nargin<4 || ~numel(c0) c0=zeros(nf,1); end
if numel(np)>1 || np(1)<1
    if nargin<3 || ~numel(nc) nc=mc; end
    f=np(:)';
    if nc==mc
        db=k*(2*[c0 cc]*cos(2*pi*(0:mc)'*f));
    else
        db=k*(2*[c0 lpccc2cc(cc,nc)]*cos(2*pi*(0:nc)'*f));
    end
else
    if nargin<3 || ~numel(nc) nc=np; end
    if nc==mc
        db=k*(2*real(rfft([c0 cc].',2*np).'));
    else
        db=k*(2*real(rfft([c0 lpccc2cc(cc,nc)].',2*np).'));
    end
    f=linspace(0,0.5,np+1);
end
if ~nargout
    plot(f,db.');
    xlabel('Normalized frequency f/f_s');
    ylabel('Gain (dB)');
end


function square_grid_test01 ( )

%*****************************************************************************80
%
%% SQUARE_GRID_TEST01 tests SQUARE_GRID using the same parameters for all dimensions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 August 2014
%
%  Author:
%
%    John Burkardt
%
  a = [ -1.0, -1.0 ];
  b = [ +1.0, +1.0 ];
  c = [ 1, 1 ];
  ns = [ 3, 3 ];

  n = ns(1) * ns(2);

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SQUARE_GRID_TEST01\n' );
  fprintf ( 1, '  Create a grid using SQUARE_GRID.\n' );
  fprintf ( 1, '  Use the same parameters in every dimension.\n' );
  fprintf ( 1, '  Number of grid points N = %d\n', n );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     I    NS     C      A         B\n' );
  fprintf ( 1, '\n' );
  for i = 1 : 2
    fprintf ( 1, '  %4d  %4d  %4d  %8.4f  %8.4f\n', i, ns(i), c(i), a(i), b(i) );
  end

  x = square_grid ( n, ns, a, b, c );
  r8mat_transpose_print ( 2, n, x, '  Grid points:' );

  return
end


% This code calculates the 3D distances between all possible pairs
% (combination of n epicenters taken 2 at a time) of earthquakes of
% a given dataset.
%
%
% Attributing the corresponding catalog to E
%
if ~exist('index', 'var')
    index = 1;
end

if index == 1
    E = newt2;
elseif index == 2
    E = ran;
elseif index == 3
    E = rann;
end
%
% Variables
%
N = size(E,1);				% N= # of events in the catalogue; E= Earthquake catalogue
pairdist = []; 			% pairdist= Vector of interevent distances
j = nchoosek(N,2)			% j= # of interevent distances calculated
pairdist = zeros(j,1);
depth = zeros(j,1);
k = 0;

Ho_Wb = waitbar(0,'Calculating the fractal dimension');
Hf_Cfig = gcf;
Hf_child = get(groot,'children');
set(Hf_child,'pointer','watch','papertype','A4');
%
% Calculation of the interevent distances in 2D plus the depths differences.
%
for i = 1:(N-1)

    lon1 = repmat(E(i,1), [(N-i),1]);
    lat1 = repmat(E(i,2), [(N-i),1]);
    depth1 = repmat(E(i,7), [(N-i),1]);
    lon2 = E((i+1):end, 1);
    lat2 = E((i+1):end, 2);
    depth2 = E((i+1):end, 7);
    pairdist(k+1:k + size(lon1, 1)) = distance(lat1,lon1,lat2,lon2);
    depth(k+1:k + size(lon1, 1)) = depth1-depth2;
    k = k + size(lon1,1);
    waitbar((0.5/(N-1))*i, Ho_Wb);

end
%
% Converts the interevent distances from degrees to kilometers and calculates
% the interevent distances in three dimensions.
%
pairdist = pairdist.*111;
pairdist = (pairdist.^2 + depth.^2).^0.5;
clear depth;
%
% Compute the correlation integral
%
d = 3;			%the embedding dimension
docorint;


% This example shows how to calculate and plot both the fundamental
% quasi-TE eigenmode and quasi-TM eigenmode of an example 3-layer
% ridge waveguide using the semivectorial eigenmode solver.

% Refractive indices:
n1 = 3.34;          % Lower cladding
n2 = 3.44;          % Core
n3 = 1.00;          % Upper cladding (air)

% Vertical dimensions:
h1 = 2.0;           % Lower cladding
h2 = 1.3;           % Core thickness
h3 = 0.5;           % Upper cladding
rh = 1.1;           % Ridge height

% Horizontal dimensions:
rw = 1.0;           % Ridge half-width
side = 1.5;         % Space on side

% Grid size:
dx = 0.0125;        % grid size (horizontal)
dy = 0.0125;        % grid size (vertical)

lambda = 1.55;      % vacuum wavelength
nmodes = 1;         % number of modes to compute

[x,y,xc,yc,nx,ny,eps,edges] = waveguidemesh([n1,n2,n3],[h1,h2,h3], ...
                                            rh,rw,side,dx,dy);

% First, consider the quasi-TE mode:

[Ex,neff] = svmodes(lambda,n2,nmodes,dx,dy,eps,'000S','EX');

fprintf(1,'neff = %.6f\n',neff);

figure(1);
contourmode(x,y,Ex);
title('Ex (TE Mode)'); xlabel('x'); ylabel('y');
for v = edges, line(v{:}); end

% Next, consider the quasi-TM mode:

[Ey,neff] = svmodes(lambda,n2,nmodes,dx,dy,eps,'000S','EY');

fprintf(1,'neff = %.6f\n',neff);

figure(2);
contourmode(x,y,Ey);
title('Ey (TM mode)'); xlabel('x'); ylabel('y');
for v = edges, line(v{:}); end


function [D]=distND(V1,V2)

D=zeros(size(V1,1),size(V2,1));

for q=1:1:size(V1,2) %For all dimensions
    A=V1(:,q); %Coordinates of first set
    B=V2(:,q); %Coordinates of second set

    AB=A(:,ones(1,size(B,1)));
    BA=B(:,ones(1,size(A,1)))';

    D=D+(AB-BA).^2;
end
D=sqrt(D);

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


function truncated_normal_a_cdf_test ( )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_A_CDF_TEST tests TRUNCATED_NORMAL_A_CDF;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 September 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRUNCATED_NORMAL_A_CDF_TEST\n' );
  fprintf ( 1, '  TRUNCATED_NORMAL_A_CDF evaluates the lower Truncated Normal CDF.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The "parent" normal distribution has\n' );
  fprintf ( 1, '    mean = mu\n' );
  fprintf ( 1, '    standard deviation = sigma\n' );
  fprintf ( 1, '  The parent distribution is truncated to\n' );
  fprintf ( 1, '  the interval [a,+oo)\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '                                                  Stored         Computed\n' );
  fprintf ( 1, '       X        Mu         S         A             CDF             CDF\n' );
  fprintf ( 1, '\n');

  n_data = 0;

  while ( 1 )

    [ n_data, mu, sigma, a, x, cdf1 ] ...
      = truncated_normal_a_cdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    cdf2 = truncated_normal_a_cdf ( x, mu, sigma, a );

    fprintf( 1, '  %8.1f  %8.1f  %8.1f  %8.1f  %14g  %14g\n', x, mu, sigma, a, cdf1, cdf2 );

  end

  return
end


function x = spgrid(n,d,options)
% SPGRID   Compute the sparse grid point coordinates
%    X = SPGRID(N,D)  Computes the sparse grid points of level N
%    and problem dimension D. The coordinate value of dimension i
%    is stored in column i of the matrix X. One row of matrix X
%    represents one grid point.
%
%    X = SPGRID(N, D, OPTIONS) computes the sparse grid points as
%    above, but with default grid type replaced by the grid type
%    specified in OPTIONS, an argument created with the SPSET
%    function. See SPSET for details.
%
%    See also SPINTERP, SPVALS, SPDIM.

% Author : Andreas Klimke
% Version: 1.3
% Date   : November 18, 2007

% Change log:
% V1.0   : September 24, 2003
%          Initial version
% V1.1   : April 20, 2004
%          Compute sequence of levels here instead of in spgridxx
%          subroutine.
% V1.2   : June 15, 2004
%          Added new grid type : Chebyshev distributed nodes
%          (at the extrema of the Chebyshev polynomials)
% V1.3   : November 18, 2007
%          Added new grid type : Gauss-Patterson

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

if nargin < 3, options = []; end
if nargin < 2, d = []; end

gridtype = spget(options, 'GridType', 'Clenshaw-Curtis');
sparseIndices = spget(options, 'SparseIndices', 'auto');
options = spset(options, 'SparseIndices', sparseIndices);

switch lower(gridtype)
 case 'clenshaw-curtis'
	if strcmpi(sparseIndices, 'off')
		gridgen = 'spgridcc';
	else
		gridgen = 'spgridccsp';
	end
 case 'maximum'
	gridgen = 'spgridm';
 case 'noboundary'
	gridgen = 'spgridnb';
 case 'chebyshev'
	if strcmpi(sparseIndices, 'off')
		gridgen = 'spgridcb';
	else
		gridgen = 'spgridcbsp';
	end
 case 'gauss-patterson'
	if strcmpi(sparseIndices, 'off')
		gridgen = 'spgridgp';
	else
		gridgen = 'spgridgpsp';
	end
 otherwise
	error('MATLAB:spinterp:badopt',['Unknown grid type ''' gridtype '''.']);
end

% Get the sequence of levels
if ~isempty(d)
	levelseq = spgetseq(n,d,options);
else
	% For internal usage: pass sequence of levels directly.
	levelseq = n;
end

x = feval(gridgen, levelseq);


function upsilon = lfmjpComputeUpsilonMatrix(gamma, sigma2, t1, t2, mode)

% LFMJPCOMPUTEUPSILONMATRIX Upsilon matrix jolt. pos. with t1, t2 limits
% FORMAT
% DESC computes a portion of the LFMJ kernel.
% ARG gamma : Gamma value for system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode : operation mode, according to the derivative (mode 0,
% derivative wrt t1, mode 1 derivative wrt t2)
% RETURN upsilon : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2010
%
% SEEALSO : lfmComputeUpsilonMatrix.F, lfmComputeH3.m

% KERN

sigma = sqrt(sigma2);
gridt1 = repmat(t1, 1, length(t2));
gridt2 = repmat(t2', length(t1), 1);
timeGrid = gridt1 - gridt2;

if mode ==0
    upsilon = gamma^2*lfmvpComputeUpsilonMatrix(gamma, sigma2, t1, t2, 0) ...
        + (4/(sqrt(pi)*sigma^3))*exp(-(timeGrid.^2)./sigma2).*...
        (timeGrid.*(gamma + 2*timeGrid/sigma2) - 1);
else
    upsilon = gamma^2*lfmvpComputeUpsilonMatrix(gamma, sigma2, t1, t2, 1) ...
        - (4/(sqrt(pi)*sigma^3))*exp(-(timeGrid.^2)./sigma2).*...
        (timeGrid.*(gamma + 2*timeGrid/sigma2) - 1) ...
        - (4/(sqrt(pi)*sigma^3))*exp(-gamma*t1)*((t2.*(gamma - 2*t2/sigma2) + 1).*...
        exp(-(t2.^2)/sigma2)).';
end


function [pos,vel,acc]=parabolicblend(t,tb,omega,thetai,thetaf)

toffset=t(1);
t=t-toffset;

tbefore=find(t<tb);
tparbegin=t(1:tbefore(end));

%%the parabolic starting blend
pos=thetai+(omega/(2*tb)).*tparbegin.^2;
vel=omega/tb.*tparbegin;
acc=omega/tb;
tparend=t(length(t)-tbefore(end)+1:end);
tlinear=t(tbefore(end)+1:end-length(tparend));
%%the linear zone
pos=[pos, pos(end)+omega/tb.*tlinear ];

%%the parabolic ending blend
k=omega/(2*tb);

pos=[pos, thetaf-k.*(t(end)-tparend).^2];
end


function [ n, a, seed ] = tree_rb_yule ( n, a, seed )

%*****************************************************************************80
%
%% TREE_RB_YULE adds two nodes to a rooted binary tree using the Yule model.
%
%  Discussion:
%
%    The Yule model is a simulation of how an evolutionary family tree
%    develops.  We start with a root node.  The internal nodes of the tree
%    are inactive and never change.  Each pendant or leaf node of the
%    tree represents a biological family that can spontaneously "fission",
%    developing two new distinct sub families.  In graphical terms, the node
%    becomes internal, with two new leaf nodes depending from it.
%
%    The tree is stored in inorder traversal form.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 June 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer N, the number of nodes in the input
%    tree.  On output, this number has been increased, usually by 2.
%
%    Input/output, integer A(*), the preorder traversal form
%    for the rooted binary tree.  The number of entries in A is N.
%
%    Input/output, integer SEED, a seed for the random number
%    generator.
%
  if ( n <= 0 )
    n = 1;
    a(1) = 0;
    return
  end
%
%  Count the expected number of leaves, which are the 0 values.
%
  nleaf = floor ( ( n + 1 ) / 2 );
%
%  Choose a random number between 1 and NLEAF.
%
  ileaf = i4_uniform_ab ( 1, nleaf, seed );
%
%  Locate leaf number ILEAF.
%
  j = 0;
  jleaf = 0;
  for i = 1 : n
    if ( a(i) == 0 )
      jleaf = jleaf + 1;
    end
    if ( jleaf == ileaf )
      j = i;
      break
    end
  end
%
%  Replace '0' by '100'
%
  a(n+2:-1:j+2) = a(n:-1:j);
  a(j) = 1;
  a(j+1) = 0;

  n = n + 2;

  return
end