function points=Cart2Ellipse(cartPoints,algorithm,a,f)
%%CART2ELLIPSE Convert Cartesian coordinates to ellipsoidal (latitude,
%              longitude, and altitude) coordinates.
%
%INPUTS: cartPoints A matrix of the points in ECEF Cartesian coordinates
%                   that are to be transformed into ellipsoidal
%                   coordinates. Each column of cartPoints is of the
%                   format [x;y;z].
%         algorithm This specified the algorithm to use for the conversion.
%                   Note that none work at the origin. Possible values are:
%                   0 (The default if this parameter is omitted or an empty
%                     matrix is passed and f<0.01) Use the algorithm of
%                     Olson in [1].
%                   1 Use the Algorithm of Sofair in [2], which is a
%                     modification of [3]. This will not work close to the
%                     center of the Earth.
%                   2 (The default if this parameter is omitted or an empty
%                     matrix is passed and f>=0.01) Use the algorithm of
%                     Fukushima in [4]. This should work close to the
%                     center of the Earth.
%                 a The semi-major axis of the reference ellipsoid. If this
%                   argument is omitted, the value in
%                   Constants.WGS84SemiMajorAxis is used.
%                 f The flattening factor of the reference ellipsoid. If
%                   this argument is omitted, the value in
%                   Constants.WGS84Flattening is used.
%
%OUTPUTS: points A matrix of the converted points. Each column of the
%                matrix has the format [latitude;longitude;altitude], with
%                latitude and longitude given in radians.
%
%The algorithm of Olson in [1] appears to be the most precise non-iterative
%method available for targets far above the Earth. The method of Sofair in
%[2] and [3] is also a non-iterative algorithm, but tends to have
%significantly worse accuracy for such targets. Fukushima's algorithm in
%[4] is iterative and typically converges in six or fewer iterations. It is
%set to run for a maximum number of 500 iterations. More than 6 iterations
%can be necessary when a large flattening is used and the point in question
%is near the center of the Earth. Its accuracy appears to be marginally
%better than [1] for things not near the surface of the Earth/ reference
%ellipsoid, but it is slower.
%
%REFERENCES:
%[1] D. K. Olson, "Converting Earth-centered, Earth-fixed coordinates to
%    geodetic coordinates," IEEE Transactions on Aerospace and Electronic
%    Systems, vol. 32, no. 1, pp. 473-476, Jan. 1996.
%[2] I. Sofair "Improved method for calculating exact geodetic latitude and
%    altitude revisited," Journal of Guidance, Control, and Dynamics, vol.
%    23, no. 2, p. 369, Mar. 2000.
%[3] I. Sofair, "Improved method for calculating exact geodetic latitude
%    and altitude," Journal of Guidance, Control, and Dynamics, vol. 20,
%    no. 4, pp. 824-826, Jul.-Aug. 1997.
%[4] Fukushima, T., "Transformation from Cartesian to geodetic coordinates
%    accelerated by Halley's method", Journal of Geodesy, vol. 79, no. 12,
%    pp. 689-693, Mar. 2006.
%
%October 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(f))
    f=Constants.WGS84Flattening;
end

if(nargin<3||isempty(a))
    a=Constants.WGS84SemiMajorAxis;
end

if(nargin<2||isempty(algorithm))
    if(f<0.01)
        algorithm=0;
    else
        algorithm=2;
    end
end

switch(algorithm)
    case 0%Olson's algorithm
        [lambda,phi,h]=OlsonAlg(cartPoints,a,f);

        if(any(imag(lambda)~=0)||any(imag(phi)~=0)||any(imag(phi)~=0))
            error('The point given is too close to the center of the Earth for the algorithm of Olson.')
        end

    case 1%Sofair's algorithm
        [lambda,phi,h]=SofairAlg(cartPoints,a,f);
    case 2%Fukushima's algorithm
        [lambda,phi,h]=FukishimaAlg(cartPoints,a,f);
    otherwise
        error('Unknown algorithm specified.')
end

points=[phi;lambda;h];

end

function [lambda,phi,h]=SofairAlg(cartPoints,a,f)
%%SOFAIRALG This implements the algorithm of [1], which is a modified
%           version of the algorithm of [2]. Both techniques will fail if
%           the point in question is too close to the origin (deep within
%           the Earth). If the algorithm fails, then this function wil have
%           an error.
%
%REFERENCES:
%[1] I. Sofair "Improved method for calculating exact geodetic latitude and
%    altitude revisited," Journal of Guidance, Control, and Dynamics, vol.
%    23, no. 2, p. 369, Mar. 2000.
%[2] I. Sofair, "Improved method for calculating exact geodetic latitude
%    and altitude," Journal of Guidance, Control, and Dynamics, vol. 20,
%    no. 4, pp. 824-826, Jul.-Aug. 1997.
%
%September 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numPoints=size(cartPoints,2);

%The semi-minor axis of the reference ellipsoid.
b=a*(1-f);

%The square of the first numerical eccentricity.
e2=2*f-f^2;

%The square of the second numerical eccentricity.
eps2=a^2/b^2-1;

%Allocate space for the results.
phi=zeros(1,numPoints);
lambda=zeros(1,numPoints);
h=zeros(1,numPoints);
for curPoint=1:numPoints
    %Extract the coordinates
    x0=cartPoints(1,curPoint);
    y0=cartPoints(2,curPoint);
    z0=cartPoints(3,curPoint);

    r0=sqrt(x0.^2+y0.^2);
    p=abs(z0)/eps2;
    s=r0.^2/(e2*eps2);
    q=p.^2-b.^2+s;

    lambda(curPoint)=atan2(y0,x0);

    if(q<0)
        error('The point given is too close to the center of the Earth for the algorithm of Sofair.')
    end

    u=p./sqrt(q);
    v=b^2*u.^2./q;
    P=27*v.*s./q;
    Q=(sqrt(P+1)+sqrt(P)).^(2/3);
    t=(1+Q+1./Q)/6;
    %The max command prevents finite precision problems due to
    %subtraction within the square root.
    c=max(0,u.^2-1+2*t);
    c=sqrt(c);
    w=(c-u)/2;

    %The z coordinate of the closest point projected on the ellipsoid.
    %The max command deals with precision problems when the argument
    %is nearly zero. The problems arise due to the subtraction within
    %the square root.
    z=max(0,sqrt(t.^2+v)-u.*w-t/2-1/4);
    z=sign(z0).*sqrt(q).*(w+sqrt(z));
    Ne=a*sqrt(1+eps2.*z.^2/b^2);

    %The min and max terms deals with finite precision problems.
    val=min(z*(eps2+1)./Ne,1);
    val=max(val,-1);
    phi(curPoint)=asin(val);
    h(curPoint)=r0.*cos(phi(curPoint))+z0.*sin(phi(curPoint))-a^2./Ne;
end

end

function [lambda,phi,h]=FukishimaAlg(cartPoints,a,f)
%%FUKUSHIMAALG This function implements the algorithm of [1] with minor
%              modifications.
%
%If one lets the algorithm run for an arbitrary number of iterations, there
%will generally be underflows, since the ratio of S and C matter, but both
%terms can drift by a constant factor during the iterations. Thus, after
%each iterative step, the values are normalized so that C=1 (it takes one
%division). Convergence of Fukushima's method is assumed to occur after
%six iterations.
%
%REFERENCES:
%[1] Fukushima, T., "Transformation from Cartesian to geodetic coordinates
%    accelerated by Halley's method", J.Geodesy (2006) 79: 689-693.
%
%September 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numPoints=size(cartPoints,2);

%The semi-minor axis of the reference ellipsoid.
b=a*(1-f);

%The square of the first numerical eccentricity.
e2=2*f-f^2;

ec=sqrt(1-e2);

%Allocate space for the results.
phi=zeros(1,numPoints);
lambda=zeros(1,numPoints);
h=zeros(1,numPoints);
for curPoint=1:numPoints
    %Extract the coordinates
    x0=cartPoints(1,curPoint);
    y0=cartPoints(2,curPoint);
    z0=cartPoints(3,curPoint);

    lambda(curPoint)=atan2(y0,x0);

    p=sqrt(x0^2+y0^2);
    P=p/a;
    Z=(ec/a)*abs(z0);

    S=Z;
    C=ec*P;

    %Loop until convergence. Normally, only 6 iterations or fewer is
    %required. When some points are near the surface of the Earth and f is
    %close to 1, the required number of iterations can be higher.
    for curIter=1:500
        A=sqrt(S^2+C^2);
        B=1.5*e2*S*C^2*((P*S-Z*C)*A-e2*S*C);
        F=P*A^3-e2*C^3;
        D=Z*A^3+e2*S^3;

        SNew=D*F-B*S;
        CNew=F^2-B*C;

        SOld=S;
        COld=C;

        SNew=SNew/CNew;
        if(~isfinite(SNew))
            S=SNew;
            break;
        else
            S=SNew;
            C=1;
        end

        if(S==SOld&&C==COld)
           break;
        end
    end
    Cc=ec*C;

    %If the point is along the z-axis, then SNew and CNew will
    %both be zero, leading to a non-finite result.
    if(~isfinite(S))
        phi(curPoint)=sign(z0)*(pi/2);
        h(curPoint)=abs(z0)-b;
    else
        phi(curPoint)=sign(z0)*atan(S/Cc);
        h(curPoint)=(p*Cc+abs(z0)*S-b*A)/sqrt(Cc^2+S^2);
    end
end

end

function [lambda,phi,h]=OlsonAlg(cartPoints,a,f)
%%OLSONALG This function implements the algorithm of [1], removing a test
%          for a radius being too small as the function will still work at
%          smaller radii.
%
%REFERENCES:
%[1] D. K. Olson, "Converting Earth-centered, Earth-fixed coordinates to
%   geodetic coordinates," IEEE Transactions on Aerospace and Electronic
%   Systems, vol. 32, no. 1, pp. 473-476, Jan. 1996.
%
%October 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numPoints=size(cartPoints,2);

%The square of the eccentricity.
e2=2*f-f^2;

a1=a*e2;
a2=a1^2;
a3=a1*e2/2;
a4=(5/2)*a2;
a5=a1+a3;
a6=1-e2;

%Allocate space for the results.
phi=zeros(1,numPoints);
lambda=zeros(1,numPoints);
h=zeros(1,numPoints);
for curPoint=1:numPoints
    %Extract the coordinates
    x=cartPoints(1,curPoint);
    y=cartPoints(2,curPoint);
    z=cartPoints(3,curPoint);

    zp=abs(z);
    w2=x^2+y^2;
    w=sqrt(w2);
    z2=z^2;
    r2=w2+z2;
    %The algorithm will work with points close to the origin. Thus, there
    %is no need to have a test for r being too small as is the case in [1].
    r=sqrt(r2);

    lambda(curPoint)=atan2(y,x);
    s2=z2/r2;
    c2=w2/r2;
    u=a2/r;
    v=a3-a4/r;
    if(c2>0.3)
        s=(zp/r)*(1+c2*(a1+u+s2*v)/r);
        phi(curPoint)=asin(s);
        ss=s^2;
        c=sqrt(1-ss);
    else
        c=(w/r)*(1-s2*(a5-u-c2*v)/r);
        phi(curPoint)=acos(c);
        ss=1-c^2;
        s=sqrt(ss);
    end

    g=1-e2*ss;
    rg=a/sqrt(g);
    rf=a6*rg;
    u=w-rg*c;
    v=zp-rf*s;
    f=c*u+s*v;
    m=c*v-s*u;
    p=m/(rf/g+f);
    phi(curPoint)=phi(curPoint)+p;
    h(curPoint)=f+m*p/2;
    if(z<0)
        phi(curPoint)=-phi(curPoint);
    end
end
end

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


clear all
        % Declararea variabilei simbolice k
syms k
        % Calcularea primei sume
S1=symsum(k,1,k)
        % Afisarea rezultatului nesimplificat
pretty(S1)
        % Simplificarea rezultatului
S1=simple(S1)
        % Afisarea rezultatului simplificat
pretty(S1)
        % Calcularea sumei a doua
S2=symsum(1/(k*(k+1)),1,inf)


function [m,s] = mean_std_robust(x);

x = x(:);

m = median(x);

s = median(abs(x - m))*1.4836;


function pde = Stokesdata1
%% STOKESDATA1 data for Stokes equations
%
% A simple model of colliding flow. The force f = 0, the velocity  u1 =
% 20xy^3, u2 = 5x^4-5y^4, p = 60x^2y - 20y^3.
%
% Dirichlet boundary condition is imposed.
%
% Reference: page 237 in Finite Elements and Fast Iterative Solvers with
% Applications in Incompressible Fluid Dynamics. by Howard C. Elman, David
% J. Silvester, and Andrew J. Wathen.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

pde = struct('f', 0, 'exactp', @exactp, 'exactu', @exactu,'g_D',@exactu, 'exactw', @exactw);

    % exact velocity
    function z = exactu(p)
        x = p(:,1); y = p(:,2);
        z(:,1) = 20*x.*y.^3;
        z(:,2) = 5*x.^4-5*y.^4;
    end
    % exact pressure
    function z = exactp(p)
        x = p(:,1); y = p(:,2);
        z = 60*x.^2.*y-20*y.^3-5;
    end
    % exact vorticity
    function z = exactw(p)
        x = p(:,1); y = p(:,2);
        z = 20*x.^3-60*x.*y.^2;
    end
end


function F  = ...
    check_frequency_response(x,frequency_20,frequency_100, method)
% Checks the result of optimization by plotting phase characteristics at
% optimal parameters and computing the frequencies at -pi/2 phase shift.
% The frequency responses of a nonlinear system are obtained by processing
% the pulse transient characteristics with the FFT algorithm.
% Copyright 2010 MathWorks, Inc.

% frequency_20   - frequency (Hz) at phase shift in -pi/2 at 20% input signal
% frequency_100  - frequency (Hz) at phase shift in -pi/2 at 100% input signal
% method - method to check the results, can be set to 'FFT' or 'direct'.

if strcmpi(method, 'FFT')
    model = 'actuator_freq_testrig_pulse_FFT_method';
elseif strcmpi(method, 'direct')
    model = 'actuator_freq_testrig_direct_method';
else
    error('Unspecified method');
end

load_system(model);

assignin('base','gain', x(1));
assignin('base','time_const', x(2));
assignin('base','saturation', x(3));

sim(model);

if strcmpi(method, 'FFT')

    y_20 = yout(:,2);               % Pulse transient characteristic at 20% input
    y_100 = yout(:,1);              % Pulse transient characteristic at 100% input
    fs = 1000;                      % Sampling frequency
    n = length(y_20);               % Window length = Transform length
    y_20_fft = fft(y_20,n);         % Discrete Fourier Transform
    y_100_fft = fft(y_100,n);       % Discrete Fourier Transform
    f0 = (0:n/2-1)*(fs/n);          % Shifted frequency range, positive region
    y_20_0 = fftshift(y_20_fft);    % Shifted DFT at 20% input
    y_100_0 = fftshift(y_100_fft);  % Shifted DFT at 100% input
    % Phase characteristic at 20% input for positive frequencies after unwrap
    phase_20 = unwrap(angle(y_20_0(257:end)));
    % Phase characteristic at 100% input for positive frequencies after unwrap
    phase_100 = unwrap(angle(y_100_0(257:end)));

    % Computing frequency at 90 deg phase shift by interpolation of phase
    % characteristics
    frq_20 = interp1(phase_20,f0,-pi/2);
    frq_100 = interp1(phase_100,f0,-pi/2);

    % Computing frequency at phase shift in -pi/2 by interpolation of phase
    % characteristics
    frq_20 = interp1(phase_20,f0,-pi/2);
    frq_100 = interp1(phase_100,f0,-pi/2);
    % Computing errors
    error_20 = (frequency_20 - frq_20)/frequency_20 * 100;
    error_100 = (frequency_100 - frq_100)/frequency_100 * 100;

    disp(['******************************************************************']);
    disp(['     System Characteristics with FFT at Obtained Parameters']);
    disp(['     Frequencies at -90 deg phase shift and 20% input:']);
    disp(['Target:  ', num2str(frequency_20),' Hz', ...
        '  Found:  ',num2str(frq_20),' Hz', ...
        '   Error :   ',num2str(error_20),'%']);
    disp(['     Frequencies at -90 deg phase shift and 100% input']);
    disp(['Target:  ', num2str(frequency_100),' Hz', ...
        ' Found:     ',num2str(frq_100),' Hz', ...
        '   Error:   ',num2str(error_100),'%']);

    % Plotting the phase frequency characterictics
    semilogx(f0,phase_20*180/pi,'LineWidth',3);
    hold on
    semilogx(f0,phase_100*180/pi,'r--','LineWidth',3), grid on;
    xlabel('Frequency (Hz)','FontSize',14)
    ylabel('Phase (degrees)','FontSize',14)
    title('Frequency Response (Phase)','FontWeight','Bold','FontSize',16)
    legend({'20% Input' '100% Input'},'FontSize',12);
    axis([2 40 -100 0]);
    plot(frq_100,-90,'ro','MarkerSize',10,'MarkerFaceColor','r')
    plot(frq_20,-90,'bo','MarkerSize',10,'MarkerFaceColor','b')
    hold off

else    % Direct measurement. yout contains input and output values of
        % both signals

    % Phase angle at specified frequency and input signal in 100%
    phase_100 = phase_computation(yout(:,[1,2]), tout, frequency_100,4);
    % Phase angle at specified frequency and input signal in 20%
    phase_20 = phase_computation(yout(:,[3,4]), tout, frequency_20,6);

    error_20 = (phase_20 + 90)/90 * 100;
    error_100 = (phase_100 +90)/90 * 100;

    disp(['******************************************************************']);
    disp(['     System Characteristics after Direct Measurement at Obtained Parameters']);
    disp(['     Phase shift at ',num2str(frequency_20),'Hz',' and 20% input:']);
    disp(['Target:  ',' -90 deg', ...
        '  Found:  ',num2str(phase_20),' deg', ...
        '   Error :   ',num2str(error_20),'%']);
    disp(['     Phase shift at ',num2str(frequency_100),'Hz',' and 100% input']);
    disp(['Target:  ',' -90 deg', ...
        '  Found:   ',num2str(phase_100),' deg', ...
        '   Error:   ',num2str(error_100),'%']);
    disp(['******************************************************************']);

end
end

function [phase] = phase_computation(yout, tout, freq_at_90, N)
% Function computes phase angle by processing sinusoids stored in yout

% yout(:,1)- input; yout(:,2) - output
% tout - simulation step time
% freq_at_90 - frequency at which phase angle equals -90 deg
% N - number of periods to settle the transients

% Measurement time. Time in N periods is assumed to be enough to settle
% down the transients. The input signal at this time is equal zero

t_s = 1/freq_at_90 * N;             % Start time
t_end = t_s + 1/freq_at_90;         % End time

% yout index at measurement time
for j = 1:length(tout)- 1
    if t_s >= tout(j) && t_s < tout(j+1)
        k_start = j;                % First array index after start time
        t_corr_1 = tout(j) - t_s;   % Correction due to discretization
    end
    if t_end >= tout(j) && t_end < tout(j+1)
        k_end = j-1;                  % Last array index within period
    end
end
% Computing time the output signal crosses zero
for j = k_start : k_end
    if yout(j,2) <= 0 && yout(j+1,2) > 0
        t_tab_cross = tout(j);   % First value in the table after crossing
        % Approximating real crossing time
        t_corr_2 = -yout(j,2) / (yout(j+1,2) - yout(j,2)) * ...
            (tout(j+1) - tout(j));
    end
end
% Crossing time for the output signal
t_cross = t_tab_cross + t_corr_1 + t_corr_2;
% Phase angle
phase = -(t_cross - t_s) / (1/freq_at_90) * 360;

end

% EOF


function y = goertzel_classic(x,indvec)
% GOERTZEL_CLASSIC(X,INDVEC) computes DFT of one-dimensional signal X at indices
% contained in INDVEC, using the traditional second-order Goertzel algorithm.
% The indices must be integer values from 0 to N-1, where N is the length of
% X. (Index 0 corresponds to the DC component.)
%
% The output is a column complex vector of length LENGTH(INDVEC) containing
% the desired DFT coefficients.
%
% See also: goertzel_general_shortened.

% (c) 2009-2012, Pavel Rajmic, Brno University of Technology, Czech Rep.

%% Check the input arguments
if nargin < 2
    error('Not enough input arguments')
end
if ~isvector(x) || isempty(x)
    error('X must be a nonempty vector')
end

if ~isvector(indvec) || isempty(indvec)
    error('INDVEC must be a nonempty vector')
end
if ~isreal(indvec)
    error('INDVEC must contain real numbers')
end
% if ~isinteger(indvec)
%     error('INDVEC must contain only integer values')
% end

lx = length(x);
x = reshape(x,lx,1); %forcing x to be column

%% Initialization
no_freq = length(indvec); %number of frequencies to compute
y = zeros(no_freq,1); %memory allocation for the output coefficients

%% Computation via second-order system
% loop over the particular frequencies
for cnt_freq = 1:no_freq

    %for a single frequency:
    %a/ precompute the constants
    pik_term = 2*pi*(indvec(cnt_freq))/(lx);
    cos_pik_term2 = cos(pik_term) * 2;
    cc = exp(-1i*pik_term); % complex constant
    %b/ state variables
    s0 = 0;
    s1 = 0;
    s2 = 0;
    %c/ 'main' loop
    for ind = 1:lx %number of loops is the same as the length of signal
        %new state
        s0 = x(ind) + cos_pik_term2*s1 - s2;  % (*)
        %shifting the state variables
        s2 = s1;
        s1 = s0;
    end
    %d/ final computations
    s0 = cos_pik_term2*s1 - s2; %correspond to one extra performing of (*), where we set x(N+1)=0
    y(cnt_freq) = s0 - s1*cc; %resultant complex DFT coefficient
end


function K = rbfhKernCompute(kern, x1, x2)

% RBFHKERNCOMPUTE Compute the RBFH kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the radial basis function heat
% kernel given inputs associated with rows and columns.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x1 : the input matrix associated with the rows of the kernel.
% ARG x2 : the input matrix associated with the columns of the kernel.
% RETURN K : the kernel matrix computed at the given points.
%
% FORMAT
% DESC computes the kernel matrix for the radial basis function heat
% kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x1 : input data matrix in the form of a design matrix.
% RETURN K : the kernel matrix computed at the given points.
%
% SEEALSO : rbfhKernParamInit, kernCompute,
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 3
    x2 = x1;
end

if size(x1, 2) ~= 2 || size(x2, 2) ~= 2
    error('Input can only have two columns');
end

% Split the domain into time domain and spatial domain and account for
% missing values. If there are no missing values the computation of the
% kernel is a pointwise prodruct, otherwise it is a kronecker product.
t1 = x1(x1(:,1)~=Inf,1);
t2 = x2(x2(:,1)~=Inf,1);
s1 = x1(x1(:,2)~=Inf,2);
s2 = x2(x2(:,2)~=Inf,2);
if (length(t1) == length(s1)) && (length(t2) == length(s2))
    ut1 = unique(t1);
    ut2 = unique(t2);
    us1 = unique(s1);
    us2 = unique(s2);
    if (length(ut1)*length(us1) == length(t1)) && ...
        (length(ut2)*length(us2) == length(t2))
        t1 = ut1; s1 = us1; t2 = ut2; s2 = us2;
        isPointwise = false;
    else
        isPointwise = true;
    end
else
    isPointwise = false;
end

kern.rbf.inverseWidth = kern.inverseWidthTime;
Kt = rbfKernCompute(kern.rbf, t1, t2);
kern.rbf.inverseWidth = kern.inverseWidthSpace;
Ks = rbfKernCompute(kern.rbf, s1, s2);

if isPointwise
   K = Kt.*Ks;
else
   K = kron(Kt, Ks);
end


function y=sigpower(x)

% y=sigpower(x)
%
% Return the average signal power of signal x or mean(abs(x).^2);

% Copyright 2012 Evrytania LLC (http://www.evrytania.com)
%
% Written by James Peroulas <james@evrytania.com>
%
% 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/>.

error(nargchk(1,1,nargin));

y=sum(real(x).*real(x)+imag(x).*imag(x))/length(x);


function w = ymdf_to_weekday_common ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_WEEKDAY_COMMON returns the weekday of a Common YMDF date.
%
%  Discussion:
%
%    The "common" calendar is meant to be the calendar which is Julian up to
%    day JED = 2299160, and Gregorian from day JED = 2299161 and after.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, integer W, is the week day number of the date, with
%    1 for Sunday, through 7 for Saturday.
%
  jed = ymdf_to_jed_common ( y, m, d, f );

  [ w, f2 ] = jed_to_weekday ( jed );

  return
end


function [ point_num, edge_num, face_num, face_order_max ] = ...
  truncated_octahedron_size_3d ( )

%*****************************************************************************80
%
%% TRUNCATED_OCTAHEDRON_SIZE_3D gives "sizes" for a truncated octahedron in 3D.
%
%  Discussion:
%
%    The truncated octahedron is "space-filling".
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer POINT_NUM, the number of points.
%
%    Output, integer EDGE_NUM, the number of edges.
%
%    Output, integer FACE_NUM, the number of faces.
%
%    Output, integer FACE_ORDER_MAX, the maximum order of any face.
%
  point_num = 24;
  edge_num = 36;
  face_num = 14;
  face_order_max = 6;

  return
end


function ccff_asymptotic ( n_min, n_inc, n_max, f, f_integral )

%*****************************************************************************80
%
%% CCFF_ASYMPTOTIC computes asymptotic errors for the Legendre integral.
%
%  Discussion:
%
%    The Legendre integral being approximated is:
%      integral ( -1 <= x <= +1 ) f(x) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N_MIN, N_INC, N_MAX, the minimum, increment, and maximum
%    for the number of points in the rule.
%
%    Input, function pointer F(x), returns the value of the integrand
%    at the N points X.
%
%    Input, real F_INTEGRAL, the exact value of the integral.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N  |Quad error|\n' );
  fprintf ( 1, '\n' );
  for n = n_min : n_inc : n_max
    [ x, w ] = ccff ( n );
    fx = f ( x );
    q = w' * fx;
    e = abs ( f_integral - q );
    fprintf ( 1, '  %4d  %8.2e\n', n, e );
  end

  return
end


 function [Cone,EndPlate1,EndPlate2] = Cone(hAxis,X1,X2,R,n,cyl_color,closed,lines)
%
% This function constructs a cylinder connecting two center points
%
% Usage :
% [Cone,EndPlate1,EndPlate2] = Cone(X1,X2,R,n,cyl_color,closed,lines)
%
% Cone-------Handle of the cone
% EndPlate1------Handle of the Starting End plate
% EndPlate2------Handle of the Ending End plate
% X1 and X2 are the 3x1 vectors of the two points
% R is the radius of the cylinder/cone R(1) = start radius, R(2) = end radius
% n is the no. of elements on the cylinder circumference (more--> refined)
% cyl_color is the color definition like 'r','b',[0.52 0.52 0.52]
% closed=1 for closed cylinder or 0 for hollow open cylinder
% lines=1 for displaying the line segments on the cylinder 0 for only
% surface
%
% Typical Inputs
% X1=[10 10 10];
% X2=[35 20 40];
% r=[1 5];
% n=20;
% cyl_color='b';
% closed=1;
%
% NOTE: There is a MATLAB function "cylinder" to revolve a curve about an
% axis. This "Cylinder" provides more customization like direction and etc

% Calculating the length of the Cone
length_cyl=norm(X2-X1);

% Creating 2 circles in the YZ plane
t=linspace(0,2*pi,n)';
xa2=R(1)*cos(t);
xa3=R(1)*sin(t);
xb2=R(2)*cos(t);
xb3=R(2)*sin(t);

% Creating the points in the X-Direction
x1=[0 length_cyl];

% Creating (Extruding) the cylinder points in the X-Directions
xx1=repmat(x1,length(xa2),1);
xx2=[xa2 xb2];%xx2=repmat(x2,1,2);
xx3=[xa3 xb3];%xx3=repmat(x3,1,2);

% Drawing two filled cirlces to close the cylinder
if closed==1
    EndPlate1=fill3(xx1(:,1),xx2(:,1),xx3(:,1),'r', 'Parent', hAxis);
    EndPlate2=fill3(xx1(:,2),xx2(:,2),xx3(:,2),'r', 'Parent', hAxis);
end

% Plotting the cylinder along the X-Direction with required length starting
% from Origin
Cone=mesh(hAxis,xx1,xx2,xx3, 'Parent', hAxis);

% Defining Unit vector along the X-direction
unit_Vx=[1 0 0];

% Calulating the angle between the x direction and the required direction
% of Cone through dot product
angle_X1X2=acos( dot( unit_Vx,(X2-X1) )/( norm(unit_Vx)*norm(X2-X1)) )*180/pi;

% Finding the axis of rotation (single rotation) to roate the Cone in
% X-direction to the required arbitrary direction through cross product
axis_rot=cross([1 0 0],(X2-X1) );

% Rotating the plotted Cone and the end plate circles to the required
% angles
if angle_X1X2~=0 % Rotation is not needed if required direction is along X
    rotate(Cone,axis_rot,angle_X1X2,[0 0 0])
    if closed==1
        rotate(EndPlate1,axis_rot,angle_X1X2,[0 0 0])
        rotate(EndPlate2,axis_rot,angle_X1X2,[0 0 0])
    end
end

% Till now Cone has only been aligned with the required direction, but
% position starts from the origin. so it will now be shifted to the right
% position
if closed==1
    set(EndPlate1,'XData',get(EndPlate1,'XData')+X1(1))
    set(EndPlate1,'YData',get(EndPlate1,'YData')+X1(2))
    set(EndPlate1,'ZData',get(EndPlate1,'ZData')+X1(3))

    set(EndPlate2,'XData',get(EndPlate2,'XData')+X1(1))
    set(EndPlate2,'YData',get(EndPlate2,'YData')+X1(2))
    set(EndPlate2,'ZData',get(EndPlate2,'ZData')+X1(3))
end
set(Cone,'XData',get(Cone,'XData')+X1(1))
set(Cone,'YData',get(Cone,'YData')+X1(2))
set(Cone,'ZData',get(Cone,'ZData')+X1(3))

% Setting the color to the Cone and the end plates
set(Cone,'AmbientStrength',1,'FaceColor',cyl_color,'FaceLighting','gouraud', 'Parent', hAxis);%,'EdgeColor','none')
if closed==1
    set([EndPlate1 EndPlate2],'AmbientStrength',1,'FaceColor',cyl_color,'FaceLighting','gouraud', 'Parent', hAxis);%,'EdgeColor','none')
else
    EndPlate1=[];
    EndPlate2=[];
end

% If lines are not needed making it disapear
if lines==0
    set(Cone,'EdgeAlpha',0)
end

%shading faceted % faceted flat interp;
%camlight;
%light;
%lighting gouraud; %flat gouraud phong none
% material shiny; %shiny dull metal
%colormap(bone)

%camlight  headlight;
%light('Style','local','Position',[720 0 500]);
%light('Style','local','Position',[0 480 500]);


% Rounds a scalar, matrix or vector to a specified number of decimal places
% Format is roundoff(number,decimal_places)

function y = roundoff(number,decimal_places)

[INeg,JNeg] = find( number<0 ); % Negative numbers

if ~isempty(INeg)
   IndNeg = sub2ind(size(number),INeg,JNeg);
   Number = abs(number);
else
   Number = number;
end

decimals = 10.^decimal_places;
y1 = fix(decimals * Number + 0.5)./decimals;

if ~isempty(INeg)
   y1(IndNeg) = -y1(IndNeg);
end

y = y1;


function [thr, q, obj, im] = gmthr_seg(im, nobj, nsubs)
% GMTHR_SEG  Segment an image estimating threshold as intersection of two
% Gaussians from Gaussian mixture model
%
% THR = gmthr_seg(IM)
%
%   IM is an input n-dim image. This function assumes that IM contains a
%   darker object over a brighter background.
%
%   THR is a scalar with the estimated threshold value between dark
%   voxels (object) and lighter voxels (background). A Gaussian mixture
%   model is fitted to the image intensities, and the intersection point
%   between the Gaussian maxima is computed. The object in the image is
%   segmented using this intersection value as the segmentation threshold.
%
%   If the object and the background are too similar compared to the number
%   of samples in the image (i.e. the Gaussians intersect outside of the
%   interval between the Gaussian maxima), then this method cannot provide
%   a threshold to separate object and background. In that case, THR is
%   returned as NaN. This is the case, for example, if the image only
%   contains background, or only object voxels.
%
% [THR, Q, OBJ, BW] = gmthr_seg(IM, NOBJ, NSUBS)
%
%   NOBJ is a scalar. Only the largest NOBJ are kept in the segmentation.
%   This last step is useful to remove segmentation noise. By default,
%   NOBJ=1.
%
%   NSUBS is a scalar. For large images, the variance estimate will be too
%   small for the Gaussian mixture fitting function, which will return an
%   error. This problem can be solved doing a random subsampling of the
%   image to estimate the Gaussian mixture model. NSUBS is the subsampling
%   factor. E.g. NSUBS=100 will randomly sample numel(IM)/NSUBS voxels in
%   the image. By default, NSUBS=1 and no subsampling is performed.
%
%   Q is a quality measure of the threshold. Q takes values in [0, 1].
%   Values close to 0 mean that both Gaussians have a lot of overlap, so
%   the threshold between object and background cannot be trusted very
%   much. Values close to 1 mean that both Gaussians are well separated,
%   and the threshold value can be trusted to provide a good segmentation.
%
%   OBJ is the Gaussian mixture object. See help('gmdistribution.fit') for
%   details. The mean and variance of the Gaussians can be extracted as
%   obj.mu and obj.Sigma, respectively.
%
%   BW is an output segmentation mask, where voxels == true correspond to
%   the darker object.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2012 University of Oxford
% Version: 0.5.2
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(1, 3);
nargoutchk(0, 4);

% defaults
if (nargin < 2 || isempty(nobj))
    nobj = 1;
end
if (nargin < 3 || isempty(nsubs))
    nsubs = 1;
end

% % DEBUG: approximate pdf of whole image
% [ftot, xout] = hist(im(:), 100);
% inc = xout(2) - xout(1);
% ftot = ftot / numel(im) / inc;

% compute gaussian mixture model. Note that we need to randomly subsample
% the image so that the variance of the Gaussians is not too small
% (otherwise, gmdistribution.fit() gives an error)
if (nsubs > 1)
    idx = randi(numel(im), round(numel(im)/nsubs), 1);
    obj = gmdistribution.fit(im(idx), 2);
else
    obj = gmdistribution.fit(im(:), 2);
end

% get mixture of Gaussians parameters
[mutis, idx] = min(obj.mu);
vartis = obj.Sigma(idx);
[mubak, idx] = max(obj.mu);
varbak = obj.Sigma(idx);

% % DEBUG: create Gaussian curves for display purposes
% ftis = normpdf(xout, mutis, sqrt(vartis));
% fbak = normpdf(xout, mubak, sqrt(varbak));

% compute intersection points between two gaussians
thr = intersect_gaussians(mutis, mubak, sqrt(vartis), sqrt(varbak));

% keep the one that is between both mean values
thr = thr(thr > mutis & thr < mubak);

% if there's no intersection point between the maxima, then returning a
% threshold is meaningless, and instead we return NaN. This is the case,
% e.g. if there's only background and no object
if isempty(thr)
    thr = nan;
end

% quality of the clustering measure. Integral under the tissue Gaussian
% in [thr, Inf] and integral under the background Gaussian in [-Inf, thr]:
% the sum represents the Gaussian overlap area. This overlap has a value in
% [0, 1], with 0 for a lot of overlap, and 1 for no overlap. The quality
% measure is then 1-overlap
if (nargout < 2)
    return
end
if (isnan(thr))
    q = nan;
else
    q = 1 - normcdf(2*mutis-thr, mutis, sqrt(vartis))...
        - normcdf(thr, mubak, sqrt(varbak));
end

% % DEBUG: plot histogram curves
% hold off
% plot(xout, ftot)
% hold on
% plot(xout, ftis, 'r')
% plot(xout, fbak, 'g')
% plot([thr, thr], [0 max([ftis(:); fbak(:)])], 'k')
% legend('all', 'tissue', 'background')
% xlabel('intensity')

% no need to waste time segmenting the image if the user doesn't ask for
% the output segmentation
if (nargout < 4)
    return
end

% threshold segmentation
im = (im <= thr);

%% remove segmentation noise
% we could use function bwrmsmallcomp() here, but we don't want having to
% replicate the segmentation data too many times. That could create memory
% problems for very large volumes. Instead, we have copied the code in
% bwrmsmallcomp() directly here:

% get connected components
cc = bwconncomp(im);

% keep only the largest components to remove background noise
%
% note: it's better to clear the whole image, and then add the largest
% components, than trying to delete the smaller components. The latter
% doesn't remove all the noise, for some reason.
len = cellfun(@length, cc.PixelIdxList);
[~, idx] = sort(len, 2, 'descend');
im = zeros(size(im), 'uint8');
if ~isempty(idx)
    idx = cat(1, cc.PixelIdxList{idx(1:nobj)});
    im(idx) = 1;
end


function printTensor(tensor,order)
%-fanDTasia ToolBox------------------------------------------------------------------
% This Matlab script is part of the fanDTasia ToolBox: a Matlab library for Diffusion
% Weighted MRI (DW-MRI) Processing, Diffusion Tensor (DTI) Estimation, High-order
% Diffusion Tensor Analysis, Tensor ODF estimation, Visualization and more.
%
% A Matlab Tutorial on DW-MRI can be found in:
% http://www.cise.ufl.edu/~abarmpou/lab/fanDTasia/tutorial.php
%
%-CITATION---------------------------------------------------------------------------
% If you use this software please cite the following work:
% A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion Tensors
% of any order with Symmetric Positive-Definite Constraints",
% In the Proceedings of ISBI, 2010
%
%-DESCRIPTION------------------------------------------------------------------------
% This function prints in the command line the tensor coefficients of a given tensor
% (in 3 variables) of a specific order.
%
%-USE--------------------------------------------------------------------------------
% printTensor(tensor,order);
%
% tensor: is a vector that contains all the unique coefficients of a symmetric tensor
% order: is the order of the tensor
%
%-DISCLAIMER-------------------------------------------------------------------------
% You can use this source code for non commercial research and educational purposes
% only without licensing fees and is provided without guarantee or warrantee expressed
% or implied. You cannot repost this file without prior written permission from the
% authors. If you use this software please cite the following work:
% A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion Tensors
% of any order with Symmetric Positive-Definite Constraints", In Proc. of ISBI, 2010.
%
%-AUTHOR-----------------------------------------------------------------------------
% Angelos Barmpoutis, PhD
% Computer and Information Science and Engineering Department
% University of Florida, Gainesville, FL 32611, USA
% abarmpou at cise dot ufl dot edu
%------------------------------------------------------------------------------------

    c=1;
    for i=0:order
		for j=0:order-i
			fprintf(1,'D%d%d%d = %.8f\n',i,j,order-i-j,tensor(c));
			c=c+1;
        end
    end


function [m] = cellssq(x, dim)

% [S] = CELLSSQ(X, DIM) computes the sum of squares, across all cells in x along
% the dimension dim.
%
% X should be an linear cell-array of matrices for which the size in at
% least one of the dimensions should be the same for all cells

nx = size(x);
if ~iscell(x) || length(nx)>2 || all(nx>1),
  error('incorrect input for cellssq');
end

if nargin==1,
  scx1 = cellfun('size', x, 1);
  scx2 = cellfun('size', x, 2);
  if     all(scx2==scx2(1)), dim = 2; %let second dimension prevail
  elseif all(scx1==scx1(1)), dim = 1;
  else   error('no dimension to compute sum of squares for');
  end
end

nx   = max(nx);
ssmp = cellfun(@sumsq,   x, repmat({dim},1,nx), 'UniformOutput', 0);
m    = sum(cell2mat(ssmp), dim);

function [s] = sumsq(x, dim)

s = sum(x.^2, dim);


function fpc = eci2fpc1(gast, reci, veci)

% convert inertial state vector to flight path coordinates

% input

%  gast = greenwich apparent sidereal time (radians)
%  reci = inertial position vector (kilometers)
%  veci = inertial velocity vector (kilometers/second)

% output

%  fpc(1) = east longitude (radians)
%  fpc(2) = geocentric declination (radians)
%  fpc(3) = flight path angle (radians)
%  fpc(4) = azimuth (radians)
%  fpc(5) = position magnitude (kilometers)
%  fpc(6) = velocity magnitude (kilometers/second)

% global

%  omega = inertial rotation rate (radians/second)

% Orbital Mechanics with MATLAB

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

global omega

% compute geocentric radius

rmag = norm(reci);

% compute earth-relative position vector

c(1, 1) = cos(gast);
c(1, 2) = sin(gast);
c(1, 3) = 0.0d0;

c(2, 1) = -sin(gast);
c(2, 2) = cos(gast);
c(2, 3) = 0.0d0;

c(3, 1) = 0.0d0;
c(3, 2) = 0.0d0;
c(3, 3) = 1.0d0;

recf = c * reci';

% add earth rotation effect

vtmp(1) = veci(1) + reci(2) * omega;

vtmp(2) = veci(2) - reci(1) * omega;

vtmp(3) = veci(3);

% compute relative velocity vector and magnitude

vecf = c * vtmp';

vrel = norm(vecf);

% compute east longitude and geocentric declination

xlong = atan3(recf(2), recf(1));

decl = asin(recf(3) / rmag);

% compute flight path angle and azimuth

c(1, 1) = -sin(decl) * cos(xlong);
c(1, 2) = -sin(decl) * sin(xlong);
c(1, 3) = cos(decl);

c(2, 1) = -sin(xlong);
c(2, 2) = cos(xlong);
c(2, 3) = 0.0d0;

c(3, 1) = -cos(decl) * cos(xlong);
c(3, 2) = -cos(decl) * sin(xlong);
c(3, 3) = -sin(decl);

vr = c * vecf;

fpa = asin(-vr(3) / vrel);

azimuth = atan3(vr(2), vr(1));

% flight path coordinates

fpc(1) = xlong;

fpc(2) = decl;

fpc(3) = fpa;

fpc(4) = azimuth;

fpc(5) = rmag;

fpc(6) = vrel;


function sphere_monte_carlo_test01 ( )

%*****************************************************************************80
%
%% SPHERE_MONTE_CARLO_TEST01 tests SPHERE01_SAMPLE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 January 2014
%
%  Author:
%
%    John Burkardt
%
  e_test(1:3,1:7) = [ ...
    0, 0, 0; ...
    2, 0, 0; ...
    0, 2, 0; ...
    0, 0, 2; ...
    4, 0, 0; ...
    2, 2, 0; ...
    0, 0, 4 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  Use SPHERE01_SAMPLE to estimate integrals over \n' );
  fprintf ( 1, '  the surface of the unit sphere.\n' );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '         N        1              X^2             Y^2' )
  fprintf ( 1, '             Z^2             X^4           X^2Y^2           Z^4\n' );
  fprintf ( 1, '\n' );

  n = 1;

  while ( n <= 65536 )

    [ x, seed ] = sphere01_sample ( n, seed );

    fprintf ( 1, '  %8d', n );

    for j = 1 : 7

      e(1:3,1) = e_test(1:3,j);

      value(1:n,1) = monomial_value ( 3, n, e, x );

      result = sphere01_area ( ) * sum ( value(1:n) ) / n;

    fprintf ( 1, '  %14f', result );

    end

    fprintf ( 1, '\n' );

    n = 2 * n;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '     Exact' );
  for j = 1 : 7

    e(1:3,1) = e_test(1:3,j);

    result = sphere01_monomial_integral ( e );

    fprintf ( 1, '  %14f', result );

  end
  fprintf ( 1, '\n' );

  return
end


function x = inv_triu(U)
% INV_TRIU     Invert upper triangular matrix.

x = solve_triu(U,eye(size(U)));
%x = inv(U);


function [data g] = pre_diffData(varargin)
%
% Apply a difference filter to data. Differencing is a standard operation
% to improve stationarity of a time series. A first-order difference filter
% for input X is given by Y(t) = X(t) - X(t-1). This operation can be
% applied repeatedly to obtain an nth-order difference filter [1]
%
% Inputs:
%
%   EEG:        EEG data structure
%
% Optional:     <'Name',Value> pairs
%
%     VerbosityLevel:    Verbosity level. 0 = no output, 1 = text, 2 = graphical
%                        Possible values: 0,1,2
%                        Default value  : 1
%                        Input Data Type: real number (double)
%
%     DifferencingOrder: Differencing order
%                        Number of times to difference data
%                        Input Range  : [0  10]
%                        Default value: 1
%                        Input Data Type: real number (double)
% Outputs:
%
%   EEG:        processed EEG structure
%   g:          argument specification structure
%
% See Also: pop_pre_prepData(), pre_prepData(), diff()
%
% References:
%
% [1] Mullen T (2010) The Source Information Flow Toolbox (SIFT):
%   Theoretical Handbook and User Manual. Section 6.5.1
%   Available at: http://www.sccn.ucsd.edu/wiki/Sift
%
% Author: Tim Mullen 2010, SCCN/INC, UCSD.
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

g = arg_define([0 1], varargin, ...
        arg_norep('data',mandatory), ...
        arg({'verb','VerbosityLevel'},int32(1),{int32(0) int32(1) int32(2)},'Verbosity level. 0 = no output, 1 = text, 2 = graphical'), ...
        arg({'difforder','DifferencingOrder'},1,[0 10],'Differencing order. Number of times to difference data'));

% commit data to workspace
data = g.data;
g = rmfield(g,'data');

[nchs npnt ntr] = size(data);

if g.difforder==0
    return;
end

if g.verb
    disp(['Differencing ' num2str(g.difforder) ' times...']);
end

if g.verb==2
    multiWaitbar('Differencing','Reset','Color',hlp_getNextUniqueColor);
end

datmp = zeros(nchs,npnt-g.difforder,ntr);

% difference each trial
for tr=1:ntr
    if g.verb==2
        multiWaitbar('Differencing',tr/ntr);
    end
    datmp(:,:,tr) = diff(data(:,:,tr),g.difforder,2);
end

% differencing reduces the number of datapoints by difforder
% so here we insert difforder random samples at the beginning
% of each time-series
noiseVar = 0.01*var(datmp(:));
data      = cat(2,noiseVar*randn(nchs,g.difforder,ntr),datmp);

if g.verb==2
    multiWaitbar('Differencing','Close');
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%
% 	Problem 4- Step responses

z1=[];
p1=[-1 -.2+10j  -.2-10j];
k1=10;

H=zpk(z1,p1,k1);

t=0:.1:20
y1=step(H,t)

z2=-3
p2=[-1 -.2+10j  -.2-10j];
k2=10;

[num,den]=zp2tf(z2,p2,k2);

y2=step(num,den,t)

plot(t,y1,t,y2, ':')
legend('Step1', 'Step2')


function [y,f]=v_zoomfft(x,n,m,s,d)
%V_ZOOMFFT    DTFT evaluated over a linear frequency range Y=(X,N,M,S,D)
% Inputs:
%    x    vector (or matrix)
%    n    reciprocal of normalized frequency increment (can be non-integer).
%         The frequency increment is fs/n where fs is the sample frequency
%         [default n=size(x,d)]
%    m    mumber of output points is floor(m) [default m=n]
%    s    starting frequency index (can be non-integer).
%         The starting frequency is s*fs/n [default s=0]
%    d    dimension along which to do fft [default d=first non-singleton]
%
% Outputs:
%    y       Output dtft coefficients. y has the same dimensions as x except
%            that size(y,d)=floor(m).
%    f(1,m)  normalized frequencies (1 corresponds to fs)
%
% This routine allows the evaluation of the DFT over an arbitrary range of
% frequencies; as its name implies this lets you zoom into a narrow portion
% of the spectrum.
% The DTFT of X will be evaluated along dimension D at the M frequencies
% f=fs*(s+(0:m-1))/n where fs is the sample frequency. Note that N and S
% need not be integers although M will be rounded down to an integer.
% Thus v_zoomfft(x,n,n,0,d) is equivalent to fft(x,n,d) for n>=length(x).

% [1] L.R.Rabiner,  R.W.Schafer and C.M.Rader, "The chirp z-transform algorithm"
%     IEEE Trans. Audio Electroacoustics 17 (2), 86�92 (1969).

%      Copyright (C) Mike Brookes 2007
%      Version: $Id: v_zoomfft.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent n0 k0 s0 m0 b c h g
e=size(x);
p=prod(e);
if nargin<5
    d=find(e>1);
    if ~isempty(d)
        d=d(1);
    else
        d=1;
    end
end
k=e(d);
q=p/k;
if d==1
    z=reshape(x,k,q);
else
    z=shiftdim(x,d-1);
    r=size(z);
    z=reshape(z,k,q);
end
if nargin<2 || isempty(n)
    n=k;
end
if nargin<3 || isempty(m)
    m=floor(n);
else
    m=floor(m);
end
if nargin<4 || isempty(s)
    s=0;
end
l=pow2(nextpow2(m+k-1));    % round up to next power of 2
if n==fix(n) && s==fix(s) && n<2*l && n>=k
    a=fft(z,n,1);           % quickest to do a normal fft
    y=a(1+mod(s:s+m-1,n),:);
else
    % can precaluclate all this for fixed n, k, s and m
    if isempty(b) || n~=n0 || k~=k0 || s~=s0 || m~=m0
        n0=n;
        k0=k;
        s0=s;
        m0=m;
        b=exp(1i*pi*mod((s+(1-k:m-1)').^2,2*n)/n);
        c=conj(b(k:k+m-1));
        h=fft(b,l,1);
        g=exp(-1i*pi*mod(((0:k-1)').^2,2*n)/n);
    end
    a=ifft(fft(z.*repmat(g,1,q),l,1).*repmat(h,1,q)); % calculate correlation
    y=a(k:k+m-1,:).*repmat(c,1,q);
end
if d==1
    e(d)=m;
    y=reshape(y,e);
else
    r(1)=m;
    y=shiftdim(reshape(y,r),length(e)+1-d);
end
f=(s+(0:m-1))/n;


function varargout = rodr(varargin)
% VL_RODR  Rodrigues' formula
%   R = VL_RODR(OM) where OM a 3-dimensional column vector computes the
%   Rodrigues' formula of OM, returning the rotation matrix R =
%   expm(vl_hat(OM)).
%
%   [R,DR] = VL_RODR(OM) computes also the derivative of the Rodrigues
%   formula. In matrix notation this is the expression
%
%           d(vec expm(vl_hat(OM)) )
%     dR = ----------------------.
%                  d om^T
%
%   [R,DR]=VL_RODR(OM) when OM is a 3xK matrix repeats the operation for
%   each column (or equivalently matrix with 3*K elements). In this
%   case R and DR are arrays with K slices, one per rotation.
%
%   See also: VL_IRODR(), VL_HELP().
[varargout{1:nargout}] = vl_rodr(varargin{:});


% VGG MultiView Compute Library
%
% Conversions
%   vgg_KR_from_P         - extract K, R from P such that P = K*R*[eye(3) -t]
%   vgg_F_from_P          - fundamental matrix from 2 cameras
%   vgg_P_from_F          - 2 camera matrices from fundamental matrix
%   vgg_T_from_P          - trifocal tensor from 3 cameras
%   vgg_H_from_2P_plane   - inter-image homography from 2 cameras and 3D plane
%   vgg_H_from_P_plane    - projection matrix from image onto 3D plane
%   vgg_plane_from_2P_H   - 3D plane from 2 cameras and inter-image homography
%
% Multiview tensors from image correspondences
%   vgg_H_from_x_lin             - homography from points in 2 images, linear method
%   vgg_H_from_x_nonlin          - MLE of the above, by nonlinear method
%   vgg_Haffine_from_x_MLE       - MLE of affine transformation from points in 2 images, linear
%   vgg_F_from_7pts_2img         - fundamental matrix from 7 points in 2 images
%   vgg_PX_from_6pts_3img        - cameras and world points from 6 points in 3 images
%
% Preconditioning for estimation
%   vgg_conditioner_from_image - conditioning shift+scaling from image dimensions
%   vgg_conditioner_from_pts   - conditioning shift+scaling from image points
%
% Self-calibration and similar
%   vgg_signsPX_from_x         - swaps signs of P and X so that projection scales are positive
%   vgg_selfcalib_qaffine      - quasi-affine from projective reconstruction
%   vgg_selfcalib_metric_vansq - metric from projective and 3 orthogonal principal directions and square pixels
%
% Estimation
%   vgg_X_from_xP_lin          - 3D point from image projections and cameras, linear
%   vgg_X_from_xP_nonlin       - MLE of that, non-linear method
%   vgg_line3d_from_lP_lin     - 3D line segment from image line segments and cameras, linear
%   vgg_line3d_from_lP_nonlin  - MLE of that, non-linear method
%
% 3D lines representations
%   vgg_line3d_pv_from_XY      - Pluecker vector from 2 points on the line
%   vgg_line3d_pv_from_pm      - Pluecker matrix from Pluecker vector
%   vgg_line3d_pm_from_pv      - Pluecker vector from Pluecker matrix
%   vgg_line3d_Ppv             - rearrange camera matrix to project Pluecker vector to image line
%   vgg_line3d_pv_from_2planes - Pluecker vector from 2 planes meeting in the line
%   vgg_line3d_XY_from_pm      - 2 points on 3D line from Pluecker matrix
%   vgg_line3d_XY_from_pv      - 2 points on 3D line from Pluecker vector
%  (vgg_contreps               - dual of Pluecker matrix of 3D line)
%
% Auxiliary & miscellaneous
%   vgg_get_homg    - adding row of ones
%   vgg_get_nonhomg - dividing by the final coordinates
%   vgg_projective_basis_2d
%   vgg_rms_error
%   vgg_scatter_plot_homg
%   vgg_scatter_plot


function mtx = CreateMatrixFromPrimaries(R, G, B, Wp)
%
%       mtx = CreateMatrixFromPrimaries(R, G, B, Wp)
%
%
%        Input:
%           -R: the red primary for the given color space expressed as an
%           XYZ color.
%           -G: the green primary for the given color space expressed as an
%           XYZ color.
%           -B: the blue primary for the given color space expressed as an
%           XYZ color.
%           -Wp: the white-point primary for the given color space expressed as an
%           XYZ color.
%
%        Output:
%           -mtx: a conversion matrix from XYZ color space to the color
%           space defined by the three input primaries (R, G, and B) and
%           white point (Wp).
%
%     Copyright (C) 2018  Francesco Banterle
%
%     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/>.
%

A_R = [R(1) R(2) R(3) 0 0 0 0 0 0; ...
       0 0 0 R(1) R(2) R(3) 0 0 0; ...
       0 0 0 0 0 0 R(1) R(2) R(3)];
b_R = [1; 0; 0];

A_G = [G(1) G(2) G(3) 0 0 0 0 0 0; ...
       0 0 0 G(1) G(2) G(3) 0 0 0; ...
       0 0 0 0 0 0 G(1) G(2) G(3)];
b_G = [0; 1; 0];

A_B = [B(1) B(2) B(3) 0 0 0 0 0 0; ...
       0 0 0 B(1) B(2) B(3) 0 0 0; ...
       0 0 0 0 0 0 B(1) B(2) B(3)];
b_B = [0; 0; 1];

A_Wp = [Wp(1) Wp(2) Wp(3) 0 0 0 0 0 0; ...
       0 0 0 Wp(1) Wp(2) Wp(3) 0 0 0; ...
       0 0 0 0 0 0 Wp(1) Wp(2) Wp(3)];

b_Wp = [1; 1; 1];

A = [A_R; A_G; A_B; A_Wp];
b = [b_R; b_G; b_B;  b_Wp];

mtx = A \ b;

mtx = reshape(mtx, 3, 3)';

end


function [X,freq] = absoluteValFFT(x,Fs)
N = length(x); %get the number of points
k = 0:N-1;     %create a vector from 0 to N-1
T = N/Fs;      %get the frequency interval
freq = k/T;    %create the frequency range
X = fft(x)/N*2; % normalize the data

%only want the first half of the FFT, since it is redundant
cutOff = ceil(N/2);

%take only the first half of the spectrum
X = X(1:cutOff,:,:);
X = abs(X/N);
freq = freq(1:cutOff);


function Y=EWMASTD(X,d)

%   Y=EWMASTD(X,d) returns the EWMA (Exponentially Weighted Moving Average)
%   standard deviation using the historical returns in vector X and a decay
%   factor, d.
% % ======================================================================
%
%   Author: Lorenzo Brancali
%   E-mail: lbrancali@gmail.com
%   Date:   20th Febryary 2012
%
% % ======================================================================

t=length(X);

for i=1:length(X)-1

    F(i,:)=((d^(i-1))*(X(t-i,:)-mean(X))^2);

end

Y=sqrt((1-d)*sum(F));


function chebyshev_polynomial_test05 ( )

%*****************************************************************************80
%
%% CHEBYSHEV_POLYNOMIAL_TEST05 tests T_QUADRATURE_RULE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 March 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBYSHEV_POLYNOMIAL_TEST05:\n' );
  fprintf ( 1, '  T_QUADRATURE_RULE computes the quadrature rule\n' );
  fprintf ( 1, '  associated with T(n,x);\n' );

  n = 7;
  [ x, w ] = t_quadrature_rule ( n );

  r8vec2_print ( n, x, w, '      X            W' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use the quadrature rule to estimate:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Q = Integral ( -1 <= X <= +1 ) X^E / sqrt ( 1-x^2) dx\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   E       Q_Estimate      Q_Exact\n' );
  fprintf ( 1, '\n' );

  for e = 0 : 2 * n - 1
    if ( e == 0 )
      f = ones ( n, 1 );
    else
      f(1:n) = x(1:n).^e;
    end
    q = w' * f;
    q_exact = t_integral ( e );
    fprintf ( 1, '  %2d  %14g  %14g\n', e, q, q_exact );
  end

  return
end


%MODESEEK Clustering by mode-seeking
%
% 	[LAB,J] = MODESEEK(D,K)
%
% INPUT
%   D       Distance matrix or distance dataset (square)
%   K       Number of neighbours to search for local mode (default: 10)
%
% OUTPUT
%   LAB     Cluster assignments, 1..K
%   J       Indices of modal samples
%
% DESCRIPTION
% A K-NN modeseeking method is used to assign each object to its nearest mode.
%
% REFERENCES
% 1. Cheng, Y. "Mean shift, mode Seeking, and clustering", IEEE Transactions
% on Pattern Analysis and Machine Intelligence, vol. 17, no. 8, pp. 790-799,
% 1995.
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, PRKMEANS, HCLUST, KCENTRES, PROXM

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: modeseek.m,v 1.2 2006/03/08 22:06:58 duin Exp $

function [assign,J] = modeseek (d,k)

		if (nargin < 2)
		prwarning(1,'No k supplied, assuming k = 10');
		k = 10;
  end

	[m,n] = size(d);

  if numel(k) > 1
    assign = zeros(m,numel(k));
    for j = 1:numel(k)
      assign(:,j) = feval(mfilename,d,k(j));
    end
    return
  end

  d = d'; % correction to analyse asymmetric matrices horizontally

	if (m ~= n), error('distance matrix should be square'); end
	if (k < 2),  error('neighborhood size should be at least 2'); end
	if (k > n),  error('k too large for this dataset'); end

	[d,J] = sort(+d,1);		   % Find neighbours.
	f = 1./(d(k,:)+realmin); % Calculate densities.
	J(k+1:end,:) = [];   	   % Just retain indices of neighbours.

	% Find indices of local modes in neighbourhood.
	[dummy,I] = max(reshape(f(J),size(J)));

	% Translate back to indices in all the data. N now contains the
	% index of the nearest neighbour in the K-neighbourhood.
	N = J(I+[0:k:k*(m-1)]);

	% Re-assign samples to the sample their nearest neighbour is assigned to.
	% Iterate until assignments don't change anymore. Samples that then point
	% to themselves are modes; all other samples point to the closest mode.

	M = N(N);
	while (any(M~=N))
		N = M; M = N(N);
	end

	% Use renumlab to obtain assignments 1, 2, ... and the list of unique
	% assignments (the modes).

	[assign,J] = renumlab(M');

return


function  [Z] =  MCWNNM_ADMM1( Y, NSig, Par )
% This routine solves the following weighted nuclear norm optimization problem with column weights,
%
% min_{X, Z} ||W(Y-X)||_F^2 + ||Z||_w,*  s.t.  X = Z
%
% Inputs:
%        Y      -- 3p^2 x M dimensional noisy matrix, D is the data dimension, and N is the number of image patches.
%        NSig -- 3p^2 x 1 dimensional vector of weights
%        Par   -- structure of parameters
% Output:
%        Z      -- 3p^2 x M dimensional denoised matrix

% tol = 1e-8;
if ~isfield(Par, 'maxIter')
    Par.maxIter = 10;
end
if ~isfield(Par, 'rho')
    Par.rho = 1;
end
if ~isfield(Par, 'mu')
    Par.mu = 1;
end
if ~isfield(Par, 'display')
    Par.display = true;
end
% Initializing optimization variables
% Intialize the weight matrix W
mNSig = min(NSig);
W = (mNSig+eps) ./ (NSig+eps);
% Initializing optimization variables
X = zeros(size(Y));
Z = zeros(size(Y));
A = zeros(size(Y));
%% Start main loop
iter = 0;
PatNum       = size(Y,2);
TempC  = Par.Constant * sqrt(PatNum) * mNSig^2;
% TempC  = Par.Constant * sqrt(PatNum);
% Par.rho = Par.rho * (mNSig+eps)^2;

while iter < Par.maxIter
    iter = iter + 1;

    % update X, fix Z and A
    % min_{X} ||W * Y - W * X||_F^2 + 0.5 * rho * ||X - Z + 1/rho * A||_F^2
    X = diag(1 ./ (W.^2 + 0.5 * Par.rho)) * (diag(W.^2) * Y + 0.5 * Par.rho * Z - 0.5 * A);

    % update Z, fix X and A
    % min_{Z} ||Z||_*,w + 0.5 * rho * ||Z - (X + 1/rho * A)||_F^2
    Temp = X + A/Par.rho;
    [U, SigmaTemp, V] =   svd(full(Temp), 'econ');
    [SigmaZ, svp] = ClosedWNNM(diag(SigmaTemp), 2/Par.rho*TempC, eps);
    Z =  U(:, 1:svp) * diag(SigmaZ) * V(:, 1:svp)';
    %     % check the convergence conditions
    %     stopC = max(max(abs(X - Z)));
    %     if Par.display && (iter==1 || mod(iter,10)==0 || stopC<tol)
    %         disp(['iter ' num2str(iter) ',mu=' num2str(Par.mu,'%2.1e') ...
    %             ',rank=' num2str(rank(Z,1e-4*norm(Z,2))) ',stopALM=' num2str(stopC,'%2.3e')]);
    %     end
    %     if stopC < tol
    %         break;
    %     else
    % update the multiplier A, fix Z and X
    A = A + Par.rho * (X - Z);
    Par.rho = min(1e4, Par.mu * Par.rho);
    %     end
end
return;


%  Figure 10.41      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
% fig10_41.m is a script to create Figure 10.41, the step response of the
% attitude inner loop to a 2 degree step in pitch angle.
clf;
% the total inner loop system matrix is

ftq =[-0.0064    0.0263         0  -32.2000         0;
   -0.0941   -0.6240  761.1400 -196.2000         0;
   -0.0002   -0.0015   -4.4120  -12.4800         0;
         0         0    1.0000         0         0;
         0   -1.0000         0  830.0000         0];

g =[0;
  -32.7000;
   -2.0800;
         0;
         0];
h = [0     0    -1     0     0];
j =0;
% now remove the altitude to get the inner loop pitch angle
ft=ftq(1:4,1:4)
gt=g(1:4);
ht=[0 0 0 1];
% compute the dc gain
dcgain= -ht*inv(ft)*gt
% compute the reference input in radians from 2 degrees

r=2*pi/180
t=0:.1:8;
y=step(ft,r*gt/dcgain,ht,0,1,t);
plot(t,y);
xlabel('Time (sec)');
ylabel('\theta (t) (rad)');
title(' Fig. 10.41 step response of the pitch angle inner loop ');
%grid
nicegrid


function [ Y ] = ktnormalize( Y,nrm,nnproj )
if nargin==1
    nrm=[];nnproj=[];
elseif nargin==2
    nnproj=[];
end
if isempty(nrm)
    nrm=2;
end
if isempty(nnproj)
    nnproj=true;
end

if strcmpi(class(Y),'ktensor')
    I=size(Y);
    N=numel(I);
    J=length(Y.lambda);
    switch nrm
        case 0
            for n=1:N
                ts=0:I(n):(J-1)*I(n);
                [temp idx]=max(abs(Y.U{n}));
                d=Y.U{n}(idx+ts);
                d=sign(d).*max(abs(d),eps);
                Y.U{n}=bsxfun(@rdivide,Y.U{n},d);
                Y.lambda=Y.lambda.*d';
            end
        case 1
            for n=1:N
                d=max(sum(abs(Y.U{n})),eps);
                Y.U{n}=bsxfun(@rdivide,Y.U{n},d);
                Y.lambda=Y.lambda.*d';
            end
        case 2
            for n=1:N
                d=max(sum(Y.U{n}.^2),eps);
                Y.U{n}=bsxfun(@rdivide,Y.U{n},d);
                Y.lambda=Y.lambda.*d';
            end
        otherwise
            disp('Unsupported norm.');
    end

    [temp idx]=sort(abs(Y.lambda),'descend');
    Y.lambda=Y.lambda(idx);
    for n=1:N
        Y.U{n}=Y.U{n}(:,idx);
    end

    %% nonnegative projection
    if nnproj
        ts=0:I(n):(J-1)*I(n);
        [temp idx]=max(abs(Y.U{n}));
        d=sign(Y.U{n}(idx+ts));
        Y.U{n}=bsxfun(@rdivide,Y.U{n},d);
        Y.lambda=Y.lambda.*d';
    end
end

end


function varargout = hexagonalGrid(bounds, origin, size, varargin)
%HEXAGONALGRID Generate hexagonal grid of points in the plane.
%
%   usage
%   PTS = hexagonalGrid(BOUNDS, ORIGIN, SIZE)
%   generate points, lying in the window defined by BOUNDS (=[xmin ymin
%   xmax ymax]), starting from origin with a constant step equal to size.
%   SIZE is constant and is equals to the length of the sides of each
%   hexagon.
%
%   TODO: add possibility to use rotated grid
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 06/08/2005.
%
size = size(1);
dx = 3*size;
dy = size*sqrt(3);

% consider two square grids with different centers
pts1 = squareGrid(bounds, origin + [0 0],        [dx dy], varargin{:});
pts2 = squareGrid(bounds, origin + [dx/3 0],     [dx dy], varargin{:});
pts3 = squareGrid(bounds, origin + [dx/2 dy/2],  [dx dy], varargin{:});
pts4 = squareGrid(bounds, origin + [-dx/6 dy/2], [dx dy], varargin{:});

% gather points
pts = [pts1;pts2;pts3;pts4];

% eventually compute also edges, clipped by bounds
% TODO : manage generation of edges
if nargout>1
    edges = zeros([0 4]);
    x0 = origin(1);
    y0 = origin(2);

    % find all x coordinate
    x1 = bounds(1) + mod(x0-bounds(1), dx);
    x2 = bounds(3) - mod(bounds(3)-x0, dx);
    lx = (x1:dx:x2)';

    % horizontal edges : first find y's
    y1 = bounds(2) + mod(y0-bounds(2), dy);
    y2 = bounds(4) - mod(bounds(4)-y0, dy);
    ly = (y1:dy:y2)';

    % number of points in each coord, and total number of points
    ny = length(ly);
    nx = length(lx);

    if bounds(1)-x1+dx<size
        disp('intersect bounding box');
    end

    if bounds(3)-x2<size
        disp('intersect 2');
        edges = [edges;repmat(x2, [ny 1]) ly repmat(bounds(3), [ny 1]) ly];
        x2 = x2-dx;
        lx = (x1:dx:x2)';
        nx = length(lx);
    end

    for i=1:length(ly)
        ind = (1:nx)';
        tmpEdges(ind, 1) = lx;
        tmpEdges(ind, 2) = ly(i);
        tmpEdges(ind, 3) = lx+size;
        tmpEdges(ind, 4) = ly(i);
        edges = [edges; tmpEdges];
    end

end

% process output arguments
if nargout>0
    varargout{1} = pts;

    if nargout>1
        varargout{2} = edges;
    end
end


function prob_test070 ( )

%*****************************************************************************80
%
%% TEST070 tests FACTORIAL_STIRLING, I4_FACTORIAL;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST070\n' );
  fprintf ( 1, '  FACTORIAL_STIRLING computes Stirling''s\n' );
  fprintf ( 1, '    approximate factorial function;\n' );
  fprintf ( 1, '  I4_FACTORIAL evaluates the factorial function;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  N      Stirling     N!\n' );
  fprintf ( 1, '\n' );

  for i = 0 : 20
    value = factorial_stirling ( i );
    fprintf ( 1, '  %6d  %14f  %20d\n', i, value, i4_factorial ( i ) );
  end

  return
end


%points2contour
%Tristan Ursell
%Sept 2013
%
%[Xout,Yout]=points2contour(Xin,Yin,P,direction)
%[Xout,Yout]=points2contour(Xin,Yin,P,direction,dlim)
%[Xout,Yout,orphans]=points2contour(Xin,Yin,P,direction,dlim)
%[Xout,Yout,orphans,indout]=points2contour(Xin,Yin,P,direction,dlim)
%
%Given any list of 2D points (Xin,Yin), construct a singly connected
%nearest-neighbor path in either the 'cw' or 'ccw' directions.  The code
%has been written to handle square and hexagon grid points, as well as any
%non-grid arrangement of points.
%
%'P' sets the point to begin looking for the contour from the original
%ordering of (Xin,Yin), and 'direction' sets the direction of the contour,
%with options 'cw' and 'ccw', specifying clockwise and counter-clockwise,
%respectively.
%
%The optional input parameter 'dlim' sets a distance limit, if the distance
%between a point and all other points is greater than or equal to 'dlim',
%the point is left out of the contour.
%
%The optional output 'orphans' gives the indices of the original (Xin,Yin)
%points that were not included in the contour.
%
%The optional output 'indout' is the order of indices that produces
%Xin(indout)=Xout and Yin(indout)=Yout.
%
%There are many (Inf) situations where there is no unique mapping of points
%into a connected contour -- e.g. any time there are more than 2 nearest
%neighbor points, or in situations where the nearest neighbor matrix is
%non-symmetric.  Picking a different P will result in a different contour.
%Likewise, in cases where one point is far from its neighbors, it may be
%orphaned, and only connected into the path at the end, giving strange
%results.
%
%The input points can be of any numerical class.
%
%Note that this will *not* necessarily form the shortest path between all
%the points -- that is the NP-Hard Traveling Salesman Problem, for which
%there is no deterministic solution.  This will, however, find the shortest
%path for points with a symmetric nearest neighbor matrix.
%
%see also: bwtraceboundary
%
%Example 1:  continuous points
%N=200;
%P=1;
%theta=linspace(0,2*pi*(1-1/N),N);
%[~,I]=sort(rand(1,N));
%R=2+sin(5*theta(I))/3;
%
%Xin=R.*cos(theta(I));
%Yin=R.*sin(theta(I));
%
%[Xout,Yout]=points2contour(Xin,Yin,P,'cw');
%
%figure;
%hold on
%plot(Xin,Yin,'b-')
%plot(Xout,Yout,'r-','Linewidth',2)
%plot(Xout(2:end-1),Yout(2:end-1),'k.','Markersize',15)
%plot(Xout(1),Yout(1),'g.','Markersize',15)
%plot(Xout(end),Yout(end),'r.','Markersize',15)
%xlabel('X')
%ylabel('Y')
%axis equal tight
%title(['Black = original points, Blue = original ordering, Red = new ordering, Green = starting points'])
%box on
%
%
%Example 2:  square grid
%P=1;
%
%Xin=[1,2,3,4,4,4,4,3,2,1,1,1];
%Yin=[0,0,0,0,1,2,3,3,2,2,1,0];
%
%[Xout,Yout]=points2contour(Xin,Yin,P,'cw');
%
%figure;
%hold on
%plot(Xin,Yin,'b-')
%plot(Xout,Yout,'r-','Linewidth',2)
%plot(Xout(2:end-1),Yout(2:end-1),'k.','Markersize',15)
%plot(Xout(1),Yout(1),'g.','Markersize',15)
%plot(Xout(end),Yout(end),'r.','Markersize',15)
%xlabel('X')
%ylabel('Y')
%axis equal tight
%box on
%
%Example 3:  continuous points, pathological case
%N=200;
%P=1;
%theta=linspace(0,2*pi*(1-1/N),N);
%[~,I]=sort(rand(1,N));
%R=2+sin(5*theta(I))/3;
%
%Xin=(1+rand(1,N)/2).*R.*cos(theta(I));
%Yin=(1+rand(1,N)/2).*R.*sin(theta(I));
%
%[Xout,Yout]=points2contour(Xin,Yin,P,'cw');
%
%figure;
%hold on
%plot(Xin,Yin,'b-')
%plot(Xout,Yout,'r-','Linewidth',2)
%plot(Xout(2:end-1),Yout(2:end-1),'k.','Markersize',15)
%plot(Xout(1),Yout(1),'g.','Markersize',15)
%plot(Xout(end),Yout(end),'r.','Markersize',15)
%xlabel('X')
%ylabel('Y')
%axis equal tight
%title(['Black = original points, Blue = original ordering, Red = new ordering, Green = starting points'])
%box on
%
%Example 4:  continuous points, distance limit applied
%N=200;
%P=1;
%theta=linspace(0,2*pi*(1-1/N),N);
%[~,I]=sort(rand(1,N));
%R=2+sin(5*theta(I))/3;
%R(2)=5; %the outlier
%
%Xin=(1+rand(1,N)/16).*R.*cos(theta(I));
%Yin=(1+rand(1,N)/16).*R.*sin(theta(I));
%
%[Xout,Yout,orphans,indout]=points2contour(Xin,Yin,P,'cw',1);
%
%figure;
%hold on
%plot(Xin,Yin,'b-')
%plot(Xin(orphans),Yin(orphans),'kx')
%plot(Xin(indout),Yin(indout),'r-','Linewidth',2)
%plot(Xout(2:end-1),Yout(2:end-1),'k.','Markersize',15)
%plot(Xout(1),Yout(1),'g.','Markersize',15)
%plot(Xout(end),Yout(end),'r.','Markersize',15)
%xlabel('X')
%ylabel('Y')
%axis equal tight
%title(['Black = original points, Blue = original ordering, Red = new ordering, Green = starting points'])
%box on
%

function [Xout,Yout,varargout]=points2contour(Xin,Yin,P,direction,varargin)

%check to make sure the vectors are the same length
if length(Xin)~=length(Yin)
    error('Input vectors must be the same length.')
end

%check to make sure point list is long enough
if length(Xin)<2
    error('The point list must have more than two elements.')
end

%check distance limit
if ~isempty(varargin)
    dlim=varargin{1};
    if dlim<=0
        error('The distance limit parameter must be greater than zero.')
    end
else
    dlim=-1;
end

%check direction input
if and(~strcmp(direction,'cw'),~strcmp(direction,'ccw'))
    error(['Direction input: ' direction ' is not valid, must be either "cw" or "ccw".'])
end

%check to make sure P is in the right range
P=round(P);
npts=length(Xin);

if or(P<1,P>npts)
    error('The starting point P is out of range.')
end

%adjust input vectors for starting point
if size(Xin,1)==1
    Xin=circshift(Xin,[0,1-P]);
    Yin=circshift(Yin,[0,1-P]);
else
    Xin=circshift(Xin,[1-P,0]);
    Yin=circshift(Yin,[1-P,0]);
end

%find distances between all points
D=zeros(npts,npts);
for q1=1:npts
    D(q1,:)=sqrt((Xin(q1)-Xin).^2+(Yin(q1)-Yin).^2);
end

%max distance
maxD=max(D(:));

%avoid self-connections
D=D+eye(npts)*maxD;

%apply distance contraint by removing bad points and starting over
if dlim>0
    D(D>=dlim)=-1;

    %find bad points
    bad_pts=sum(D,1)==-npts;
    orphans=find(bad_pts);

    %check starting point
    if sum(orphans==P)>0
        error('The starting point index is a distance outlier, choose a new starting point.')
    end

    %get good points
    Xin=Xin(~bad_pts);
    Yin=Yin(~bad_pts);

    %number of good points
    npts=length(Xin);

    %find distances between all points
    D=zeros(npts,npts);
    for q1=1:npts
        D(q1,:)=sqrt((Xin(q1)-Xin).^2+(Yin(q1)-Yin).^2);
    end

    %max distance
    maxD=max(D(:));

    %avoid self-connections
    D=D+eye(npts)*maxD;
else
    orphans=[];
    bad_pts=zeros(size(Xin));
end

%tracking vector (has this original index been put into the ordered list?)
track_vec=zeros(1,npts);

%construct directed graph
Xout=zeros(1,npts);
Yout=zeros(1,npts);
indout0=zeros(1,npts);

Xout(1)=Xin(1);
Yout(1)=Yin(1);
indout0(1)=1;

p_now=1;
track_vec(p_now)=1;
for q1=2:npts
    %get current row of distance matrix
    curr_vec=D(p_now,:);

    %remove used points
    curr_vec(track_vec==1)=maxD;

    %find index of closest non-assigned point
    p_temp=find(curr_vec==min(curr_vec),1,'first');

    %reassign point
    Xout(q1)=Xin(p_temp);
    Yout(q1)=Yin(p_temp);

    %move index
    p_now=p_temp;

    %update tracking
    track_vec(p_now)=1;

    %update index vector
    indout0(q1)=p_now;
end

%undo the circshift
temp1=find(~bad_pts);
indout=circshift(temp1(indout0),[P,0]);

%%%%%%% SET CONTOUR DIRECTION %%%%%%%%%%%%
%contour direction is a *global* feature that cannot be determined until
%all the points have been sequentially ordered.

%calculate tangent vectors
tan_vec=zeros(npts,3);
for q1=1:npts
    if q1==npts
        tan_vec(q1,:)=[Xout(1)-Xout(q1),Yout(1)-Yout(q1),0];
        tan_vec(q1,:)=tan_vec(q1,:)/norm(tan_vec(q1,:));
    else
        tan_vec(q1,:)=[Xout(q1+1)-Xout(q1),Yout(q1+1)-Yout(q1),0];
        tan_vec(q1,:)=tan_vec(q1,:)/norm(tan_vec(q1,:));
    end
end

%determine direction of contour
local_cross=zeros(1,npts);
for q1=1:npts
    if q1==npts
        cross1=cross(tan_vec(q1,:),tan_vec(1,:));
    else
        cross1=cross(tan_vec(q1,:),tan_vec(q1+1,:));
    end
    local_cross(q1)=asin(cross1(3));
end

%figure out current direction
if sum(local_cross)<0
    curr_dir='cw';
else
    curr_dir='ccw';
end

%set direction of the contour
if and(strcmp(curr_dir,'cw'),strcmp(direction,'ccw'))
    Xout=fliplr(Xout);
    Yout=fliplr(Yout);
end

%varargout
if nargout==3
    varargout{1}=orphans;
end

if nargout==4
    varargout{1}=orphans;
    varargout{2}=indout;
end

disp('finished')


function geometry_test049 ( )

%*****************************************************************************80
%
%% TEST049 tests PARALLELOGRAM_CONTAINS_POINT_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  ntest = 5;
%
%  In, Out, Out, Out, Out
%
  ptest(1:3,1:ntest) = [ ...
    1.0,  1.0,  0.5; ...
    3.0,  3.0,  0.0; ...
    0.5,  0.5, -0.1; ...
    0.1,  0.1,  0.5; ...
    1.5,  1.6,  0.5 ]';

  p1(1:3,1) = [ 0.0; 0.0; 0.0 ];
  p2(1:3,1) = [ 2.0; 2.0; 0.0 ];
  p3(1:3,1) = [ 1.0; 1.0; 1.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST049\n' );
  fprintf ( 1, '  PARALLELOGRAM_CONTAINS_POINT_3D determines if a point\n' );
  fprintf ( 1, '    is within a parallelogram in 3D.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '           P           Inside?\n' );
  fprintf ( 1, '\n' );

  for j = 1 : ntest

    p(1:3,1) = ptest(1:3,j);

    inside = parallelogram_contains_point_3d ( p1, p2, p3, p );

    fprintf ( 1, '  %12f  %12f  %12f  %d\n', p(1:3,1), inside );

  end

  return
end


function D = Floyd_Steinberg_Dithering(G)
% ================================================================
% FUNCTION Floyd_Steinberg Dithering Algorithm
%
% Input: G = a 8-bit grayscale / color image
% Output: D = dithered image of G's format with only values 0 and 255 of the same
% ----------------------------------------------------------------
% Demo:
%        G = imread('peppers.png');
%        D = Floyd_Steinberg_Dithering(G);
%        figure('position',[50,50,600,900]),subplot(211),imshow(G),title('Original Image');
%        subplot(212),imshow(D),title('Dithered Image');
% ----------------------------------------------------------------
% For more details about Floyd Steinberg Dithering Algorithm
% Please check
% http://en.wikipedia.org/wiki/Floyd%E2%80%93Steinberg_dithering
% ----------------------------------------------------------------
% Oct. 18th 2011
% By Yue Wu,
% Department of Electrical and Computer Engineering
% Tufts University,
% Medford, MA 02155
% ================================================================

switch size(G,3)
    case 1
    G = double(G); % convert the original image from unit8 to double
    D = zeros(size(G)); % initialize dithered image D
    [M,N] = size(G); % extract size information of the original image
    for r = 1:M
        % applying '2'-like scanning order
        % ->>>>>>>>>>>>>>>>>>>>>>>-
        %                        |
        % -<<<<<<<<<<<<<<<<<<<<<<<-
        % |
        % ->>>>>>>>>>>>>>>>>>>>>>>-
        if mod(r,2) == 0 % scan pixel from left to right
            cOrder = 1:N;
            direction = 'l2r';
        else % scan pixel from right to left
            cOrder = N:-1:1;
            direction = 'r2l';
        end
        for c = cOrder
            tP = G(r,c); % current pixel intensity
            % pick nearest intensity scale two options 0 or 255
            if tP>=128 % close to 255
                D(r,c) = 255; % pick 255
            else % close to 0
                D(r,c) = 0; % pick 0
            end
            eP = tP-D(r,c); % difference before and after selection
            % diffuse difference eP to neighbor pixels
            if r~=M % deal with none bottom rows
                switch direction
                    case 'l2r'
                        if c == 1 % left-most pixel case
                            G(r,c+1) = G(r,c+1)+eP*7/13;
                            G(r+1,c) = G(r+1,c)+eP*5/13;
                            G(r+1,c+1) = G(r+1,c+1)+eP*1/13;
                        elseif c == N % deal with the right-most pixel case
                            G(r+1,c) = G(r+1,c)+eP*5/8;
                            G(r+1,c-1) = G(r+1,c-1)+eP*3/8;
                        else % the normal case
                            G(r,c+1) = G(r,c+1)+eP*7/16;
                            G(r+1,c) = G(r+1,c)+eP*5/16;
                            G(r+1,c+1) = G(r+1,c+1)+eP*1/16;
                            G(r+1,c-1) = G(r+1,c-1)+eP*3/16;
                        end
                    case 'r2l'
                        if c == N % right-most pixel case
                            G(r,c-1) = G(r,c-1)+eP/2;
                            G(r+1,c) = G(r+1,c)+eP*3/8;
                            G(r+1,c-1) = G(r+1,c-1)+eP*1/8;
                        elseif c == 1 % left-most pixel case
                            G(r+1,c) = G(r+1,c)+eP*5/8;
                            G(r+1,c+1) = G(r+1,c+1)+eP*3/8;
                        else % normal case
                            G(r,c-1) = G(r,c-1)+eP*7/16;
                            G(r+1,c) = G(r+1,c)+eP*5/16;
                            G(r+1,c-1) = G(r+1,c-1)+eP*1/16;
                            G(r+1,c+1) = G(r+1,c+1)+eP*3/16;
                        end
                end
            else % deal with the bottom row
                switch direction
                    case 'l2r'
                        if c ~=N % normal case
                            G(r,c+1) = G(r,c+1)+eP;
                        end
                    case 'r2l'
                        if c ~= 1 % normal case
                            G(r,c-1) = G(r,c-1)+eP;
                        end
                end
            end
        end
    end
    otherwise % if the input image is not one-layer gray-scale image, then apply algorithm with respect to each layer
        for i = 1:size(G,3)
            tD = Floyd_Steinberg_Dithering(G(:,:,i));
            D(:,:,i) = tD;
        end
end

D = uint8(D); % convert double D to uint8


function val = simAssetPrice_2(TimeSeries, Period, nYears, nTrials)
% SIMASSETPRICE_2 simulates the asset prices series in a Monte Carlo
% simulation.  Is similar to simAssetPrice_orig, but removes dependence on
% the GBM object (at some expense to readibility).

[expReturn, expCov] = calcExpMoments(TimeSeries, Period);
nAssets = size(expCov,1);
StartAssetPrice = TimeSeries(end, :)';

T = cholcov(expCov);
randNums = randn(nTrials*nYears, nAssets);
randNums = permute(reshape(randNums * T, nTrials, nYears, nAssets), [2,3,1]);
returns = bsxfun(@plus, randNums, expReturn);
StartAssetPrice = repmat(StartAssetPrice', [1,1,nTrials]);
val = cumprod([StartAssetPrice; 1 + returns]);

val(val<0) = 0;

end

function [expReturn, expCov] = calcExpMoments(TimeSeries, Period)
% Estimates and annualizes the expected returns and covariances for the
% given time series
returns     = price2ret(TimeSeries);
expReturn   = mean(returns);
expCov      = cov(returns);

% Annualize monthly, weekly and daily returns and correlations
if Period == 'm'
    [expReturn, expCov] = arith2geom(expReturn, expCov, 12);
elseif Period == 'w'
    % Not implemented
elseif Period == 'd'
    % Not implemented
else
    % Do nothing
end

end


% the following matlab functions are used to calculate the Rand,
% adjusted Rand, Wallace and other partition comparison coefficients
%
%    Copyright (C) 2009  UMMI@IMM
%
%    This file is part of Comparing Partitions website <http://www.comparingpartitions.info/>.
%    Comparing Partitions website 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/>.
%
%[a,b,c,d,bc,dn,confmat,res]=PartAgreeCoef(c1,c2)
%Outputs:
%ri=rand Index
%AR = adjusted rand
%jac=jaccard
%w1- Wallace c1->c2
%w2 - Wallace c2->c1
%lar1 - Larsen c1->c2
%lar2 - Larsen c2->c1
%MH - Melia & Heckerman
%VI - variation of information
%nVI - normalized variation of information
% Francisco Pinto fpinto@fm.ul.pt

function res=PartAgreeCoef_ARonly(c1,c2)

c1=c1-min(c1)+1;
c2=c2-min(c2)+1;
n=length(c1); % number cass
ng1=max(c1);
ng2=max(c2);
%dn=n*(n-1)/2; %number of possible pairwise comparisons of cases(a+b+c+d)

%y1=pdist(c1(:),'ham');
%y2=pdist(c2(:),'ham');
%size(y1)
%size(y2)
%ad=sum((y1'==y2')); % number of pairwise concordances (matches (a) and mismatches(d))(a+d)
%bc=sum((y1'~=y2')); % number of pairwise discordances(b+c)
%Rand Index
%res.ri=ad/dn;

%a=sum((y1'==0).*(y2'==0));
%b=sum(y1'==0)-a;
%c=sum(y2'==0)-a;

%res.w1=a/sum(y1'==0);% check is =dn
%res.w1a=a/(a+b);
%res.w2a=a/(a+c);
%res.w2=a/sum(y2'==0);
%d=ad-a;

%Jaccard Index
%res.jac=a/(dn-d);

%confmat=crosstab(c1,c2);
confmat=full(sparse(c1,c2,1,ng1,ng2));

coltot=sum(confmat);
rowtot=sum(confmat')';
%summat=repmat(coltot,ng1,1)+repmat(rowtot,1,ng2);
%larsenmat=2*confmat./summat;
%res.lar1=mean(max((larsenmat'))');
%res.lar2=mean(max(larsenmat)');

%todelmat=larsenmat;
%cumval=0;
%for i=1:min([ng1;ng2])
%    [val]=max(max(todelmat)');
%    [rr,cc]=find(todelmat==val);
%        todelmat(rr(1),:)=0;
%        todelmat(:,cc(1))=0;
%        cumval=cumval+confmat(rr(1),cc(1));
%end
%res.MH=cumval/n;

%H1=-sum((rowtot/n).*log2((rowtot/n)));
%H2=-sum((coltot/n)'.*log2((coltot/n)'));
%indmat=(rowtot/n)*(coltot/n);
%nozeromat=(confmat/n)+(confmat==0);
%H12=-sum(sum((confmat/n).*log2(nozeromat)));
%MI=H1+H2-H12;
%res.VI=H1+H2-2*MI;
%res.nVI=res.VI/log2(n);

nis=sum(rowtot.^2);		%sum of squares of sums of rows
njs=sum(coltot.^2);		%sum of squares of sums of columns

t1=nchoosek(n,2);		%total number of pairs of entities
t2=sum(sum(confmat.^2));	%sum over rows & columnns of nij^2
t3=.5*(nis+njs);

%Expected index (for adjustment)
nc=(n*(n^2+1)-(n+1)*nis-(n+1)*njs+2*(nis*njs)/n)/(2*(n-1));

A=t1+t2-t3;		%no. agreements
%D=  -t2+t3;		%no. disagreements

if t1==nc
   res=0;			%avoid division by zero; if k=1, define Rand = 0
else
   res=(A-nc)/(t1-nc);		%adjusted Rand - Hubert & Arabie 1985
   %res.AR2=(ad-nc)/(dn-nc); % (a+d-nc)/(a+b+c+d-nc)
end


% ***************************************************************************
% Linear Interpolation
% ***************************************************************************
% Author: Chaobin
% Email:  chaubyZou@163.com
% Date: October 2020
% ***************************************************************************
% Language: Matlab
% Also available in: Python
% Required library: None
% ***************************************************************************

classdef LinearInterpolation < handle
    properties
        name = 'linear interpolation';
        q_via = [];
        t_via = [];
    end

    methods
        % crate the objective
        % name: string
        % q_via: N x 3 array
        % t_via: N x 1 array
        function obj = LinearInterpolation(name, q_via, t_via)
            obj.name = name;
            obj.q_via = q_via;
            obj.t_via = t_via;
            if size(q_via(:,1)) ~= length(t_via)
                error('The q_via and t_via must have a same length');
            end
        end

        % linar interpolation with two data points
        % q0: the first data point
        % q1: the second data point
        % t0: the time of the first data point
        % t1: the time of the second data point
        % a0, a1: parameters
        function [a0, a1] = linear(obj, q0, q1, t0, t1)
            if abs(t0 - t1) < 1e-6
                error('t0 and t1 must be different');
            end
            a0 = q0;
            a1 = (q1 - q0)/(t1 - t0);
        end

        % linar interpolation for all data points
        % t: specified time
        % q: 1 x 3 array, output of the interpolation at time t
        function q = getPosition(obj, t)
            if (t < obj.t_via(1)) || (t > obj.t_via(end))
                error('The specific time error, time ranges error');
            end

            j = find(obj.t_via >= t, 1, 'first'); % find the index of t1
            if j == 1
                i = 1;
                j = 2;
            else
                i = j-1;
            end

            % position
            q0 = obj.q_via(i,1);
            t0 = obj.t_via(i);
            q1 = obj.q_via(j,1);
            t1 = obj.t_via(j);
            [a0, a1] = obj.linear(q0, q1, t0, t1);
            q(1, 1) = a0 + a1*(t - t0);

            % velocity
            q(1, 2) = a1;

            % acceleration
            q(1, 3) = 0; % for linear model, the acceleration is infinite, here we set to zero
        end
    end
end


function [SE3] = SE3MatrixFromComponents( x, y, z, r, p, yaw )

% SE3MatrixFromComponents - build a 4x4 matrix representing an SE(3) transform
%
% [SE3] = SE3MatrixFromComponents( x, y, z, r, p, yaw )
%
% INPUTS:
%   x, y, z: translation
%   r, p, yaw: rotation in Euler angle representation
%
% OUTPUTS:
%   SE3: 4x4 matrix representing the SE(3) transform

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright (c) 2016 University of Oxford
% Authors:
%  Geoff Pascoe (gmp@robots.ox.ac.uk)
%  Will Maddern (wm@robots.ox.ac.uk)
%
% This work is licensed under the Creative Commons
% Attribution-NonCommercial-ShareAlike 4.0 International License.
% To view a copy of this license, visit
% http://creativecommons.org/licenses/by-nc-sa/4.0/ or send a letter to
% Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % Allow passing of a single 6-element vector
  if nargin == 1
    y = x(2);
    z = x(3);
    r = x(4);
    p = x(5);
    yaw = x(6);
    x = x(1);
  end

  % Convert euler angles to rotation matrices
  R_x = [
    1, 0, 0;
    0, cos(r), -sin(r);
    0, sin(r), cos(r) ];

  R_y = [
    cos(p), 0, sin(p);
    0, 1, 0;
    -sin(p), 0, cos(p) ];

  R_z = [
    cos(yaw), -sin(yaw), 0;
    sin(yaw), cos(yaw), 0;
    0, 0, 1];

  R = R_z * R_y * R_x;

  SE3 = [R [x; y; z]; zeros(1,3), 1];

end


%TREXP Matrix exponential for so(3) and se(3)
%
% For so(3)::
%
% R = TREXP(OMEGA) is the matrix exponential (3x3) of the so(3) element OMEGA that
% yields a rotation matrix (3x3).
%
% R = TREXP(OMEGA, THETA) as above, but so(3) motion of THETA*OMEGA.
%
% R = TREXP(S, THETA) as above, but rotation of THETA about the unit vector S.
%
% R = TREXP(W) as above, but the so(3) value is expressed as a vector W
% (1x3) where W = S * THETA. Rotation by ||W|| about the vector W.
%
% For se(3)::
%
% T = TREXP(SIGMA) is the matrix exponential (4x4) of the se(3) element SIGMA that
% yields a homogeneous transformation  matrix (4x4).
%
% T = TREXP(SIGMA, THETA) as above, but se(3) motion of SIGMA*THETA, the
% rotation part of SIGMA (4x4) must be unit norm.
%
% T = TREXP(TW) as above, but the se(3) value is expressed as a twist vector TW
% (1x6).
%
% T = TREXP(TW, THETA) as above, but se(3) motion of TW*THETA, the
% rotation part of TW (1x6) must be unit norm.
%
% Notes::
% - Efficient closed-form solution of the matrix exponential for arguments that are
%   so(3) or se(3).
% - If THETA is given then the first argument must be a unit vector or a
%   skew-symmetric matrix from a unit vector.
% - Angle vector argument order is different to ANGVEC2R.
%
% References::
% - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p42-43.
% - Mechanics, planning and control, Park & Lynch, Cambridge, 2017.
%
% See also ANGVEC2R, TRLOG, TREXP2, SKEW, SKEWA, Twist.

%## 3d homogeneous differential

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math
function T = trexp(S, theta)

    if ishomog(S) || isvec(S,6)
        % input is se(3)

        if nargin == 1
            % twist vector 1x6 or augmented skew matrix 4x4
            if isvec(S,6)
                % it's a twist vector
                S = skewa(S);
            end
            T = expm(S);
        else
            % se(3) plus twist
            if all(size(S) == 4)
                % it's se(3) matrix
                [skw,v] = tr2rt(S);
            else
                % it's a twist vector
                v = S(1:3); v= v(:);
                skw = skew(S(4:6));
            end

            % use an efficient solution
            R = trexp(skw, theta);
            t = (eye(3,3)*theta + (1-cos(theta))*skw + (theta-sin(theta))*skw^2)*v;

            T = rt2tr(R,t);
        end
    elseif isrot(S) || isvec(S,3)
        % input is so(3)

        if isrot(S)
            % input is 3x3 skew symmetric
            w = vex(S);
        elseif isvec(S)
            % input is a 3-vector
             w = S;
        end

        % for a zero so(3) return unit matrix, theta not relevant
        if norm(w) < 10*eps
            T = eye(3,3);
            return;
        end

        if nargin == 1
            %  theta is not given, extract it
            theta = norm(w);
            w = unit(w);
        else
            % theta is given
            assert(isunit(w), 'SMTB:trexp:badarg',  'w must be a unit twist');
        end

        S = skew(w);

        T = eye(3,3) + sin(theta)*S + (1-cos(theta))*S^2;

    else
        error('SMTB:trexp:badarg', 'first argument must be so(3), 3-vector, se(3) or 6-vector');
    end
end


function [ type, typeCost ] = assignVanishingType( lines, vp, tol, area )
%ASSIGNVANISHINGTYPE Summary of this function goes here
%   Detailed explanation goes here
if nargin<=3
    area = 10;
end

numLine = size(lines,1);
numVP = size(vp, 1);
typeCost = zeros(numLine, numVP);
% perpendicular
for vid = 1:numVP
    cosint = dot( lines(:,1:3), repmat( vp(vid,:), [numLine 1]), 2);
    typeCost(:,vid) = asin(abs(cosint));
end
% infinity
for vid = 1:numVP
    valid = true(numLine,1);
    for i = 1:numLine
        us = lines(i,5);
        ue = lines(i,6);
        u = [us;ue]*2*pi-pi;
        v = computeUVN(lines(i,1:3), u, lines(i,4));
        xyz = uv2xyzN([u v], lines(i,4));
        x = linspace(xyz(1,1),xyz(2,1),100);
        y = linspace(xyz(1,2),xyz(2,2),100);
        z = linspace(xyz(1,3),xyz(2,3),100);
        xyz = [x' y' z'];
        xyz = xyz ./ repmat(sqrt(sum(xyz.^2,2)),[1 3]);
        ang = acos( abs(dot(xyz, repmat(vp(vid,:), [100 1]), 2)));
        valid(i) = ~any(ang<area*pi/180);
    end
    typeCost(~valid,vid) = 100;
end

[I, type] = min(typeCost,[],2);
type(I>tol) = numVP+1;

end


t = 0:0.1:10;
x = exp(-0.2*t) .* cos(2*t);
y = exp(-0.2*t) .* sin(2*t);
plot3(x,y,t,'LineWidth',2);
title('\bfThree-Dimensional Line Plot');
xlabel('\bfx');
ylabel('\bfy');
zlabel('\bftime');
grid on;


function [samples,hists] = SampleDGAnyMarginal(gammas,Lambda,supports,Nsamples)

% [samples,hists]=SampleDGAnyMarginal(gammas,Lambda,supports,Nsamples)
%   Generate samples for a Multivariate Discretized Gaussian with parameters
%   "gammas" and "Lambda" and "supports". The number of samples generated is "Nsamples"
%
%   input and output arguments are as described in "DGAnyMarginal"
%
% Usage:
%
% Code from the paper: 'Generating spike-trains with specified
% correlations', Macke et al., submitted to Neural Computation
%
% www.kyb.mpg.de/bethgegroup/code/efficientsampling

d=size(Lambda,1);

if isempty(supports)
    for k=1:d
        supports{k}=[0:numel(gammas{k})-1];
    end
end

cc=chol(Lambda);

B=randn(Nsamples,d)*cc;

for k=1:d
    [hists{k},dd]=histc(B(:,k),[-inf;gammas{k};inf]);
    hists{k}=hists{k}/Nsamples;
    samples(:,k)=supports{k}(dd);
    hists{k}=hists{k}(1:max(1,end-2));
end


function spherical_harmonic_test02 ( )

%*****************************************************************************80
%
%% SPHERICAL_HARMONIC_TEST02 tests SPHERICAL_HARMONIC and SPHERICAL_HARMONIC_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERICAL_HARMONIC_TEST02:\n' );
  fprintf ( 1, '  SPHERICAL_HARMONIC evaluates the\n' );
  fprintf ( 1, '    spherical harmonic function.\n' );
  fprintf ( 1, '  SPHERICAL_HARMONIC_VALUES returns some exact values.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      L       M    THETA   PHI   YR   YI\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, l, m, theta, phi, yr, yi ] = spherical_harmonic_values ( n_data );

    if ( n_data == 0 )
      break
    end

    [ c, s ] = spherical_harmonic ( l, m, theta, phi );

    fprintf ( 1, '  %6d  %6d  %6f  %6f  %12f  %12f\n', ...
      l, m, theta, phi, yr, yi );
    fprintf ( 1, '                                      %12f  %12f\n', ...
      c(l+1), s(l+1) );

  end

  return
end