function gt=gabtight(varargin)
%GABTIGHT  Canonical tight window of Gabor frame
%   Usage:  gt=gabtight(a,M,L);
%           gt=gabtight(g,a,M);
%           gt=gabtight(g,a,M,L);
%           gd=gabtight(g,a,M,'lt',lt);
%
%   Input parameters:
%         g     : Gabor window.
%         a     : Length of time shift.
%         M     : Number of modulations.
%         L     : Length of window. (optional)
%         lt    : Lattice type (for non-separable lattices).
%   Output parameters:
%         gt    : Canonical tight window, column vector.
%
%   `gabtight(a,M,L)` computes a nice tight window of length *L* for a
%   lattice with parameters *a*, *M*. The window is not an FIR window,
%   meaning that it will only generate a tight system if the system
%   length is equal to *L*.
%
%   `gabtight(g,a,M)` computes the canonical tight window of the Gabor frame
%   with window *g* and parameters *a*, *M*.
%
%   The window *g* may be a vector of numerical values, a text string or a
%   cell array. See the help of |gabwin| for more details.
%
%   If the length of *g* is equal to *M*, then the input window is assumed to
%   be a FIR window. In this case, the canonical dual window also has
%   length of *M*. Otherwise the smallest possible transform length is
%   chosen as the window length.
%
%   `gabtight(g,a,M,L)` returns a window that is tight for a system of
%   length *L*. Unless the input window *g* is a FIR window, the returned
%   tight window will have length *L*.
%
%   `gabtight(g,a,M,'lt',lt)` does the same for a non-separable lattice
%   specified by *lt*. Please see the help of |matrix2latticetype| for a
%   precise description of the parameter *lt*.
%
%   If $a>M$ then an orthonormal window of the Gabor Riesz sequence with
%   window *g* and parameters *a* and *M* will be calculated.
%
%   Examples:
%   ---------
%
%   The following example shows the canonical tight window of the Gaussian
%   window. This is calculated by default by |gabtight| if no window is
%   specified:::
%
%     a=20;
%     M=30;
%     L=300;
%     gt=gabtight(a,M,L);
%
%     % Simple plot in the time-domain
%     figure(1);
%     plot(gt);
%
%     % Frequency domain
%     figure(2);
%     magresp(gt,'dynrange',100);
%
%   See also:  gabdual, gabwin, fir2long, dgt

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: TEST_DGT
%   REFERENCE: OK

%% ------------ decode input parameters ------------

if nargin<3
    error('%s: Too few input parameters.',upper(mfilename));
end;

if numel(varargin{1})==1
  % First argument is a scalar.

  a=varargin{1};
  M=varargin{2};

  g='gauss';

  varargin=varargin(3:end);
else
  % First argument assumed to be a vector.

  g=varargin{1};
  a=varargin{2};
  M=varargin{3};

  varargin=varargin(4:end);
end;

definput.keyvals.L=[];
definput.keyvals.lt=[0 1];
definput.keyvals.nsalg=0;
[flags,kv,L]=ltfatarghelper({'L'},definput,varargin);

%% ------ step 2: Verify a, M and L
if isempty(L)
    if isnumeric(g)
        % Use the window length
        Ls=length(g);
    else
        % Use the smallest possible length
        Ls=1;
    end;

    % ----- step 2b : Verify a, M and get L from the window length ----------
    L=dgtlength(Ls,a,M,kv.lt);

else

    % ----- step 2a : Verify a, M and get L

    Luser=dgtlength(L,a,M,kv.lt);
    if Luser~=L
        error(['%s: Incorrect transform length L=%i specified. Next valid length ' ...
               'is L=%i. See the help of DGTLENGTH for the requirements.'],...
              upper(mfilename),L,Luser);
    end;

end;

%% ----- step 3 : Determine the window

[g,info]=gabwin(g,a,M,L,kv.lt,'callfun',upper(mfilename));

if L<info.gl
  error('%s: Window is too long.',upper(mfilename));
end;

R=size(g,2);

% -------- Are we in the Riesz sequence of in the frame case

scale=1;
if a>M*R
  % Handle the Riesz basis (dual lattice) case.
  % Swap a and M, and scale differently.
  scale=sqrt(a/M);
  tmp=a;
  a=M;
  M=tmp;
end;

% -------- Compute -------------

if kv.lt(2)==1
    % Rectangular case

    if (info.gl<=M) && (R==1)

        % Diagonal of the frame operator
        d = gabframediag(g,a,M,L);
        gt=g./sqrt(long2fir(d,info.gl));

    else

        % Long window case

        % Just in case, otherwise the call is harmless.
        g=fir2long(g,L);

        gt=comp_gabtight_long(g,a,M)*scale;

    end;

else

    % Just in case, otherwise the call is harmless.
    g=fir2long(g,L);

    if (kv.nsalg==1) || (kv.nsalg==0 && kv.lt(2)<=2)

        mwin=comp_nonsepwin2multi(g,a,M,kv.lt,L);

        gtfull=comp_gabtight_long(mwin,a*kv.lt(2),M)*scale;

        % We need just the first vector
        gt=gtfull(:,1);

    else

        [s0,s1,br] = shearfind(L,a,M,kv.lt);

        if s1 ~= 0
            p1 = comp_pchirp(L,s1);
            g = p1.*g;
        end

        b=L/M;
        Mr = L/br;
        ar = a*b/br;

        if s0 == 0
            gt=comp_gabtight_long(g,ar,Mr);
        else
            p0=comp_pchirp(L,-s0);
            g = p0.*fft(g);
            gt=comp_gabtight_long(g,L/Mr,L/ar)*sqrt(L);
            gt = ifft(conj(p0).*gt);
        end

        if s1 ~= 0
            gt = conj(p1).*gt;
        end

    end;

    if (info.gl<=M) && (R==1)
        gt=long2fir(gt,M);
    end;

end;

% --------- post process result -------

if isreal(g) && (kv.lt(2)<=2)
  % If g is real and the lattice is either rectangular or quinqux, then
  % the output is known to be real.
  gt=real(gt);
end;

if info.wasrow
  gt=gt.';
end;


% example of usage of package bruteForceBinaryPairwiseMex
%
% Anton Osokin,  03.04.2015

numNodes = 4;
numEdges = 5;

% [Dp(1), Dp(2)] - unary terms
unaryPotentials=[
    0,16;
    0,13;
    20,0;
    4,0
];

% [p, q, Vpq(1, 1), Vpq(1, 2), Vpq(2, 1), Vpq(2, 2)] - pairwise terms
pairwisePotentials=[
    1,2,0,10,4,0;
    1,3,0,12,-1,0;
    3,2,0,9,-1,0;
    2,4,0,14,0,0;
    3,4,0,0,7,0
    ];

[energy, labels] = bruteForceBinaryPairwiseMex(unaryPotentials, pairwisePotentials);

% % correct answer:
% energy = 22;
% labels = [1; 1; 2; 1];

if ~isequal(energy, 22)
    warning('Wrong value of the energy!')
end
if ~isequal(labels, [1; 1; 2; 1])
    warning('Wrong values of the labels!')
end


function varargout = cpower(varargin)
%CPOWER Power of SDPVAR variable with convexity knowledge
%
% CPOWER is recommended if your goal is to obtain
% a convex model, since the function CPOWER is implemented
% as a so called nonlinear operator. (For p/q ==2 you can
% however just as well use the overloaded power)
%
% t = cpower(x,p/q)
%
% For negative p/q, the operator is convex.
% For positive p/q with p>q, the operator is convex.
% For positive p/q with p<q, the operator is concave.
%
% A domain constraint x>0 is automatically added if
% p/q not is an even integer.
%
% Note, the complexity of generating the conic representation
% of these variables are O(2^L) where L typically is the
% smallest integer such that 2^L >= min(p,q)

switch class(varargin{1})

    case 'double'
        varargout{1} = power(varargin{1},varargin{2});
    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.
        X = varargin{1};
        if isreal(X)
            dim = size(X);
            X = reshape(X,prod(dim),1);
            y = [];
            for i = 1:prod(dim)
                y = [y;yalmip('define',mfilename,extsubsref(X,i),varargin{2})];
            end
            y = reshape(y,dim);
            varargout{1} = y;
        else
            error('CPOWER can only be applied to real vectors.');
        end

    case 'char' % YALMIP send 'model' when it wants the epigraph or hypograph
        if isequal(varargin{1},'graph')
            t = varargin{2}; % Second arg is the extended operator variable
            X = varargin{3}; % Third arg and above are the args user used when defining t.
            p = varargin{4};

            if p>0
                [p,q] = rat(abs(p));
                F = pospower(X,t,p,q);
                if p>q
                    convexity = 'convex';
                    monotonicity = 'increasing';
                else
                    convexity = 'concave';
                    monotonicity = 'decreasing';
                end
            else
                [p,q] = rat(abs(p));
                F = negpower(X,t,p,q);
                convexity = 'convex';
                monotonicity = 'decreasing';
            end

            varargout{1} = F;
            varargout{2} = struct('convexity',convexity,'monotonicity',monotonicity,'definiteness','positive','model','graph');
            varargout{3} = X;
        end
    otherwise
end

function F = pospower(x,t,p,q)
if p>q
    l = ceil(log2(abs(p)));
    r = 2^l-p;
    y = [ones(r,1)*x;ones(q,1)*t;ones(2^l-r-q,1)];
    F = detset(x,y);
else
    l = ceil(log2(abs(q)));
    y = [ones(p,1)*x;ones(2^l-q,1)*t;ones(q-p,1)];
    F = detset(t,y);
end

function F = negpower(x,t,p,q)
l = ceil(log2(abs(p+q)));
p = abs(p);
q = abs(q);
y = [ones(2^l-p-q,1);ones(p,1)*x;ones(q,1)*t];
F = detset(1,y);


function pass = test_separableFormat( pref )
% Test code for computing separable format of PDOs.
%
% Alex Townsend, September 2014.

% Constant coefficient, Laplace:
N = chebop2(@(x,y,u) diff(u, 2, 1) + diff(u, 2, 2), [-1 1 -1 1]);
[U, S, V] = chebop2.separableFormat( N );
pass(1)  = checkCoeffs(N.coeffs, U, S, V);

% Constant coefficient, helmholtz:
N = chebop2(@(x,y,u) diff(u, 2, 1) + diff(u, 2, 2) + 100*u, [-1 1 -1 1]);
[U, S, V] = chebop2.separableFormat( N );
pass(2)  = checkCoeffs(N.coeffs, U, S, V);

% Rank 3 PDO:
N = chebop2(@(x,y,u) diff(u, 2, 1) + diff(u, 2, 2) + (10*x + y).*u, [-1 1 -1 1]);
[U, S, V] = chebop2.separableFormat( N );
pass(3)  = checkCoeffs(N.coeffs, U, S, V);

% Highly oscillatory variable coefficient:
N = chebop2(@(x,y,u) diff(u, 2, 1) + diff(u, 2, 2) + cos(100*x).*u, [-1 1 -1 1]);
[U, S, V] = chebop2.separableFormat( N );
pass(4)  = checkCoeffs(N.coeffs, U, S, V);

% Helmholtz with gravity:
N = chebop2(@(x,y,u) diff(u, 2, 1) + diff(u, 2, 2) + (10 + y).*u, [-1 1 -1 1]);
[U, S, V] = chebop2.separableFormat( N );
pass(5)  = checkCoeffs(N.coeffs, U, S, V);

% On non-standard domain:
N = chebop2(@(x,y,u) diff(u,2,1) + diff(u,2,2) + (10+y).*u, ...
    [-1 pi/3 -sqrt(3)/2 1.1]);
[U, S, V] = chebop2.separableFormat( N );
pass(6)  = checkCoeffs(N.coeffs, U, S, V);

% Variable coefficients on higher derivatives:
N = chebop2(@(x,y,u) (1+x.^2).*diff(u,2,1) + diff(u,2,2) + u, ...
    [-1 pi/3 -sqrt(3)/2 1.1]);
[U, S, V] = chebop2.separableFormat( N );
pass(7)  = checkCoeffs(N.coeffs, U, S, V);

% Heat equation:
N = chebop2(@(x,y,u) diff(u,1,1) - diff(u, 2, 2), [-1 1 0 10]);
[U, S, V] = chebop2.separableFormat( N );
pass(8) = checkCoeffs(N.coeffs, U, S, V);

% Wave equation:
N = chebop2(@(x,y,u) diff(u, 1, 2) - x.^2.*diff(u, 2, 2) + y.*u, [-1 1 0 10]);
[U, S, V] = chebop2.separableFormat( N );
pass(9) = checkCoeffs(N.coeffs, U, S, V);

% complex scalar:
N = chebop2(@(x,y,u) 1i*diff(u, 1, 1));
[U, S, V] = chebop2.separableFormat( N );
pass(10) = checkCoeffs(N.coeffs, U, S, V);

% Complex heat equation?
N = chebop2(@(x,y,u) 1i*diff(u,1,1) + (1+1i)*diff(u,1,2));
[U, S, V] = chebop2.separableFormat( N );
pass(11) = checkCoeffs(N.coeffs, U, S, V);

% Time-independent Schrodinger (complex coefficients):
hb = 0.0256;
N = chebop2(@(x,y,u) 1i*hb*diff(u, 1, 1) + hb^2*diff(u, 2, 2) - x.^2.*u, ...
    [-1 1 0 10]);
[U, S, V] = chebop2.separableFormat( N );
pass(12) = checkCoeffs(N.coeffs, U, S, V);

end

function bol = checkCoeffs( A, cellU, S, cellV )
% CheckCoeffs  debugging script.
% Alex Townsend, September 2014.
for jj = 1:size(cellU, 1)
    for kk = 1:size(cellV, 1)
        a = cellU{jj,1} * S(1,1) * cellV{kk,1}.';
        for r = 2:size(cellU,2)
            a = a + cellU{jj,r} * S(r,r) * cellV{kk,r}.';
        end
        store{jj,kk} = a;
        err(jj,kk) = norm( a - A{jj,kk} );
    end
end
if ( norm( err ) < 1e-8 )
    bol = 1;
else
    bol = err;
end
% debug plot:
% sp = 1;
% M = size(A, 1);
% N = size(A, 2);
% for jj = 1:M
%     for kk = 1:N
%         subplot(M,N,sp), plot(store{jj,kk} )
%         sp = sp + 1;
%     end
% end
end


% We consider a switching Kalman filter of the kind studied
% by Zoubin Ghahramani, i.e., where the switch node determines
% which of the hidden chains we get to observe (data association).
% e.g., for n=2 chains
%
% X1 -> X1
% | X2 -> X2
% \ |
%  v
%  Y
%  ^
%  |
%  S
%
% Y is a gmux (multiplexer) node, where S switches in one of the parents.
% We differ from Zoubin by not connecting the S nodes over time (which
% doesn't make sense for data association).
% Indeed, we assume the S nodes are always observed.
%
%
% We will track 2 objects (points) moving in the plane, as in BNT/Kalman/tracking_demo.
% We will alternate between observing them.

nobj = 2;
N = nobj+2;
Xs = 1:nobj;
S = nobj+1;
Y = nobj+2;

intra = zeros(N,N);
inter = zeros(N,N);
intra([Xs S], Y) =1;
for i=1:nobj
  inter(Xs(i), Xs(i))=1;
end

Xsz = 4; % state space = (x y xdot ydot)
Ysz = 2;
ns = zeros(1,N);
ns(Xs) = Xsz;
ns(Y) = Ysz;
ns(S) = n;

bnet = mk_dbn(intra, inter, ns, 'discrete', S, 'observed', [S Y]);

% For each object, we have
% X(t+1) = F X(t) + noise(Q)
% Y(t) = H X(t) + noise(R)
F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];
H = [1 0 0 0; 0 1 0 0];
Q = 1e-3*eye(Xsz);
%R = 1e-3*eye(Ysz);
R = eye(Ysz);

% We initialise object 1 moving to the right, and object 2 moving to the left
% (Here, we assume nobj=2)
init_state{1} = [10 10 1 0]';
init_state{2} = [10 -10 -1 0]';

for i=1:nobj
  bnet.CPD{Xs(i)} = gaussian_CPD(bnet, Xs(i), 'mean', init_state{i}, 'cov', 1e-4*eye(Xsz));
end
bnet.CPD{S} = root_CPD(bnet, S); % always observed
bnet.CPD{Y} = gmux_CPD(bnet, Y, 'cov', repmat(R, [1 1 nobj]), 'weights', repmat(H, [1 1 nobj]));
% slice 2
eclass = bnet.equiv_class;
for i=1:nobj
  bnet.CPD{eclass(Xs(i), 2)} = gaussian_CPD(bnet, Xs(i)+N, 'mean', zeros(Xsz,1), 'cov', Q, 'weights', F);
end

% Observe objects at random
T = 10;
evidence = cell(N, T);
data_assoc = sample_discrete(normalise(ones(1,nobj)), 1, T);
evidence(S,:) = num2cell(data_assoc);
evidence = sample_dbn(bnet, 'evidence', evidence);

% plot the data
true_state = cell(1,nobj);
for i=1:nobj
  true_state{i} = cell2num(evidence(Xs(i), :)); % true_state{i}(:,t) = [x y xdot ydot]'
end
obs_pos = cell2num(evidence(Y,:));
figure(1)
clf
hold on
styles = {'rx', 'go', 'b+', 'k*'};
for i=1:nobj
  plot(true_state{i}(1,:), true_state{i}(2,:), styles{i});
end
for t=1:T
  text(obs_pos(1,t), obs_pos(2,t), sprintf('%d', t));
end
hold off
relax_axes(0.1)

% Inference
ev = cell(N,T);
ev(bnet.observed,:) = evidence(bnet.observed, :);

engines = {};
engines{end+1} = jtree_dbn_inf_engine(bnet);
%engines{end+1} = scg_unrolled_dbn_inf_engine(bnet, T);
engines{end+1} = pearl_unrolled_dbn_inf_engine(bnet);
E = length(engines);

inferred_state = cell(nobj,E); % inferred_state{i,e}(:,t)
for e=1:E
  engines{e} = enter_evidence(engines{e}, ev);
  for i=1:nobj
    inferred_state{i,e} = zeros(4, T);
    for t=1:T
      m = marginal_nodes(engines{e}, Xs(i), t);
      inferred_state{i,e}(:,t) = m.mu;
    end
  end
end
inferred_state{1,1}
inferred_state{1,2}

% Plot results
figure(2)
clf
hold on
styles = {'rx', 'go', 'b+', 'k*'};
nstyles = length(styles);
c = 1;
for e=1:E
  for i=1:nobj
    plot(inferred_state{i,e}(1,:), inferred_state{i,e}(2,:), styles{mod(c-1,nstyles)+1});
    c = c + 1;
  end
end
for t=1:T
  text(obs_pos(1,t), obs_pos(2,t), sprintf('%d', t));
end
hold off
relax_axes(0.1)


% Code to perform Raycasting on a Supplied Map to return simulated Lidar
% Range values.
% % Author- Shikhar Shrestha, IIT Bhubaneswar

% Input parameters are x-position, y-position, Binary image of map, number
% of rays to cast, Lidar range in cm.
% % Returned Range values are obtained on scanning clockwise.
function [range]=castrays(xc,yc,map,n,lidarrange)
% % Conversion factor from pixels to centimeters.
pixeltocm=1;
if map(ceil(yc),ceil(xc))==1
    range=zeros(n,1);
    return;
end
figure(2);
hold off;
imshow(map);
hold on;
plot(xc,yc,'b*');

if nargin==3
    n=20;
    lidarrange=500;
end

if nargin==4
lidarrange=500;
end

range=zeros(n,1);
thetastep=360/n;

for i=1:n
theta=thetastep*i;
r=linspace(0,lidarrange,1000);
x=xc+(r*cosd(theta));
y=yc+(r*sind(theta));
% Removing points out of map
temp=[];
for k=1:numel(x)
    if x(k)>size(map,2) || y(k)>size(map,1) || x(k)<=0 || y(k)<=0
        temp=[temp;k];
    end
end
x(temp)=[];
y(temp)=[];
figure(2);
plot(x,y,'r');
% Computing Intersections
xint=round(x);
yint=round(y);
% Correcting zero map indexing
for l=1:numel(xint)
    if xint(l)==0
        xint(l)=1;
    end
    if yint(l)==0
        yint(l)=1;
    end
end

b=[];
for j=1:numel(xint)
b=[b;map(yint(j),xint(j))];
end
ind=find(b==1);

if ~isempty(ind)
xb=x(ind(1));
yb=y(ind(1));
figure(2);
plot(xb,yb,'g*');
dist=sqrt((xc-xb).^2 + (yc-yb).^2);
range(i)=dist;
end
pause(0.000001);
end
% Converting to mm from pixels.
range=range*10*pixeltocm;
% % % % % % % range=flipud(range);


function order = hexagon_unit_set ( rule )

%*****************************************************************************80
%
%% HEXAGON_UNIT_SIZE sizes a quadrature rule inside the unit hexagon in 2D.
%
%  Integration region:
%
%    The definition is given in terms of THETA, the angle in degrees of the
%    vector (X,Y).  The following six conditions apply, respectively,
%    between the bracketing values of THETA of 0, 60, 120, 180, 240,
%    300, and 360.
%
%                              0 <= Y <= - SQRT(3) * X + SQRT(3)
%                              0 <= Y <=                 SQRT(3)/2
%                              0 <= Y <=   SQRT(3) * X + SQRT(3)
%      - SQRT(3) * X - SQRT(3)   <= Y <= 0
%                    - SQRT(3)/2 <= Y <= 0
%        SQRT(3) * X - SQRT(3)   <= Y <= 0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Abramowitz and Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964.
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, integer RULE, the rule desired.
%      1, 1 point,  degree 1;
%      2, 4 points, degree 3;
%      3, 7 points, degree 3;
%      4, 7 points, degree 5;
%
%    Output, integer ORDER, the order of the desired rule.
%    If RULE is not legal, then ORDER is returned as -1.
%
  if ( rule == 1 )

    order = 1;
%
%  Stroud rule H2:3-1.
%
  elseif ( rule == 2 )

    order = 4;
%
%  Stroud rule H2:3-2.
%
  elseif ( rule == 3 )

    order = 7;
%
%  Stroud rule H2:5-1.
%
  elseif ( rule == 4 )

    order = 7;

  else

    order = -1;

  end

  return
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%                  1         , -2<=t<=2
% 	Graph of f(t)= 0         , 2<t<5
%                t*sin(4pi*t), 5<=t<=8

t1=-2:.1:2;
t2=2.1:.1:4.9;
t3=5:.1:8;

f1=ones(size(t1));
f2=zeros(size(t2));
f3=t3.*sin(4*pi*t3);

t=[t1 t2 t3];
f=[f1 f2 f3];

plot(t,f)
title('Multi-part function f(t)')


clear all; close all; clc;

load catData_w.mat
load dogData_w.mat
CD=[dog_wave cat_wave];
[u,s,v]=svd(CD-mean(CD(:)));

xtrain=[v(1:60,2:2:4); v(81:140,2:2:4)];
label=[ones(60,1); -1*ones(60,1)];
test=[v(61:80,2:2:4); v(141:160,2:2:4)];

class=classify(test,xtrain,label);
subplot(4,1,3), bar(class,'FaceColor',[.6 .6 .6],'EdgeColor','k'), axis off

truth=[ones(20,1); -1*ones(20,1)];
E=100-sum(0.5*abs(class-truth))/40*100

plot(v(1:80,2),v(1:80,4),'ro','Linewidth',[1],'MarkerEdgeColor','k','MarkerFaceColor',[0 1 0.2],'MarkerSize',8), hold on
plot(v(81:end,2),v(81:end,4),'bo','Linewidth',[1],'MarkerEdgeColor','k','MarkerFaceColor',[0.9 0 1],'MarkerSize',8), set(gca,'Fontsize',[15])

figure(2)
for j=1:2
    subplot(2,2,j)
    U3=flipud(reshape(u(:,2*j),32,32));
    pcolor(U3), colormap(hot), axis off
end

%%
load dogData.mat
load catData.mat
CD=double([dog cat]);
[u,s,v]=svd(CD-mean(CD(:)));

xtrain=[v(1:60,2:2:4); v(81:140,2:2:4)];
label=[ones(60,1); -1*ones(60,1)];
test=[v(61:80,2:2:4); v(141:160,2:2:4)];

class=classify(test,xtrain,label);
subplot(4,1,4), bar(class,'FaceColor',[.6 .6 .6],'EdgeColor','k'), axis off

%% cross-validate

for jj=1:100;
    r1=randperm(80); r2=randperm(80);
    ind1=r1(1:60); ind2=r2(1:60)+60;
    ind1t=r1(61:80); ind2t=r2(61:80)+60;

    xtrain=[v(ind1,2:2:4); v(ind2,2:2:4)];
    test=[v(ind1t,2:2:4); v(ind2t,2:2:4)];

    label=[ones(60,1); -1*ones(60,1)];
    truth=[ones(20,1); -1*ones(20,1)];
    class=classify(test,xtrain,label);
    E(jj)=sum(abs(class-truth))/40*100;
end

figure(4)
bar(E,'FaceColor',[.6 .6 .6],'EdgeColor','k'), axis([0 100 0 100])
hold on
plot([0 100],[mean(E) mean(E)],'r:','Linewidth',[2])
set(gca,'Fontsize',[15])

%%
close all
figure(1)
load catData_w.mat
load dogData_w.mat
CD=[dog_wave cat_wave];
[u,s,v]=svd(CD-mean(CD(:)));

for j=1:2
    subplot(2,2,j)
    plot(v(1:80,2),v(1:80,4),'ro','Linewidth',[1],'MarkerEdgeColor','k',...
        'MarkerFaceColor',[0 1 0.2],...
        'MarkerSize',8), hold on   % [0.49 1 .63]
    plot(v(81:end,2),v(81:end,4),'bo','Linewidth',[1],'MarkerEdgeColor','k',...
        'MarkerFaceColor',[0.9 0 1],...
        'MarkerSize',8)
    set(gca,'Fontsize',[15]), hold on
end

xtrain=[v(1:60,2:2:4); v(81:140,2:2:4)];
label=[ones(60,1); -1*ones(60,1)];
test=[v(61:80,2:2:4); v(141:160,2:2:4)];

subplot(2,2,1)
[class,~,~,~,coeff]=classify(test,xtrain,label);
K = coeff(1,2).const;
L = coeff(1,2).linear;
f = @(x,y) K + [x y]*L;
h2 = ezplot(f,[-.15 0.25 -.3 0.2]);
set(h2,'Linecolor','k','Linewidth',[2])
xlabel(''),ylabel(''),title('')

subplot(2,2,2)
[class,~,~,~,coeff]=classify(test,xtrain,label,'quadratic');
K = coeff(1,2).const;
L = coeff(1,2).linear;
Q = coeff(1,2).quadratic;
f = @(x,y) K + [x y]*L + sum(([x y]*Q) .* [x y], 2);
h2 = ezplot(f,[-.15 0.25 -.3 0.2]);
set(h2,'Linecolor','k','Linewidth',[2])
xlabel(''),ylabel(''),title('')

%set(gca,'Linewidth',[2])
%set(gcf,'LineStyle','k-')

%% figure 0
figure(5), subplot(2,2,1)

load catData_w.mat
load dogData_w.mat
CD=[dog_wave cat_wave];
[u,s,v]=svd(CD-mean(CD(:)));

plot(v(1:80,2),v(1:80,4),'ro','Linewidth',[1],'MarkerEdgeColor','k',...
    'MarkerFaceColor',[0 1 0.2],...
    'MarkerSize',8), hold on   % [0.49 1 .63]
plot(v(81:end,2),v(81:end,4),'bo','Linewidth',[1],'MarkerEdgeColor','k',...
    'MarkerFaceColor',[0.9 0 1],...
    'MarkerSize',8)
axis off

x=linspace(-0.1,-.4,200); %x2=cos(theta)*x + sin(theta)*y;
y=0.2*exp(-1000*(x2+0.25).^2);
theta=-20; Ar=[cos(theta) sin(theta); -sin(theta) cos(theta)];
%x2=cos(theta)*x + sin(theta)*y;
%y2=-sin(theta)*x + cos(theta)*y;
hold on
fill(x,y,[240 194 224]/255,'MarkerEdgeColor',[0.9 0 1],'Linewidth',[2])
set(gca,'Fontsize',[15])

plot([-0.05  0.25],[-.5 -0.25],'k')

clear x
clear y
figure(6)
x=-7:0.01:7;
y=exp(-(x-2).^2);
yn=y/trapz(x,y);
y2=exp(-0.5*(x+2).^2);
yn2=y2/trapz(x,y2);
fill(x,yn,[240 194 224]/255,'Linewidth',[2]), axis off
hold on
fill(x,yn2,[179 255 179]/255,'Linewidth',[2]),
alpha(0.5)
set(gca,'Linewidth',[2])

figure(7)
y=exp(-0.3*(x-0.6).^2);
yn=y/trapz(x,y);
y2=exp(-0.2*(x+0.5).^2);
yn2=y2/trapz(x,y2);
fill(x,yn,[240 194 224]/255,'Linewidth',[2]), axis off
hold on
fill(x,yn2,[179 255 179]/255,'Linewidth',[2]),
alpha(0.5)
set(gca,'Linewidth',[2])


function  [Z, sigma] =  MCWNNM_ADMM_NL1( 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
%        sigma -- the noise standard deviation

% 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

% Initializing optimization variables
% Intialize the weight matrix W
mNSig = min(NSig);
W = (mNSig+eps) ./ (NSig+eps);

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);
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
sigma = sqrt( mean( reshape(mean((Y - Z).^2, 2)', [Par.ps2 Par.ch])) )';
return;


function g = dftcorr(f, w)
%DFTCORR 2-D correlation in the frequency domain.
%   G = DFTCORR(F, W) performs the correlation of a mask, W, with
%   image F. The output, G, is the correlation image, of class
%   double. The output is of the same size as F. When, as is
%   generally true in practice, the mask image is much smaller than
%   G, wraparound error is negligible if W is padded to size(F).

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.4 $  $Date: 2003/10/26 23:20:44 $

[M, N] = size(f);
f = fft2(f);
w = conj(fft2(w, M, N));
g = real(ifft2(w.*f));


% Set axes limit.

clear all;
addpath('../lib');

%% lets plot 3 cycles of 50Hz AC voltage
f = 50;
Vm = 10;
phi = 0;

% generate the signal
t = [0:0.0001:3/f];
th = 2*pi*f*t;
v = Vm*sin(th+phi);

figure;
plot(t*1E3, v);

%% plot now
opt.XLabel = 'Time, t (ms)'; % xlabel
opt.YLabel = 'Voltage, V (V)'; %ylabel
opt.XLim = [0, 40]; % set x axis limit
opt.YLim = [-11, 11]; % set y axis limit

% Save? comment the following line if you do not want to save
opt.FileName = 'plotAxisLimit.jpg';

% create the plot
setPlotProp(opt);


function boundary = p05_boundary_nearest ( m, n, point )

%*****************************************************************************80
%
%% P05_BOUNDARY_NEAREST returns a nearest boundary point in problem 05.
%
%  Discussion:
%
%    The given input point need not be inside the region.
%
%    In some cases, more than one boundary point may be "nearest",
%    but only one will be returned.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real POINT(M,N), the coordinates of the points.
%
%    Output, real BOUNDARY(M,N), points on the boundary
%    that are nearest to each point.
%
  center1 = [  0.0, 0.0 ];
  center2 = [ -0.4, 0.0 ];
  r1 = 1.0;
  r2 = 0.55;

  for j = 1 : n

    dist_min = r8_huge ( );
%
%  Distance to the semicircle #1.
%
    if ( center1(2) <= point(2,j) )

      norm = sqrt ( ( point(1,j) - center1(1) ).^2 ...
                  + ( point(2,j) - center1(2) ).^2 );

      if ( 0.0 == norm )

        pn(1) = center1(1) + r1 * sqrt ( 0.5 );
        pn(2) = center1(2) + r1 * sqrt ( 0.5 );

      else

        pn(1) = center1(1) + ( point(1,j) - center1(1) ) / norm;
        pn(2) = center1(2) + ( point(2,j) - center1(2) ) / norm;

      end

    elseif ( point(1,j) <= center1(1) )

      pn(1) =  center1(1) - r1;
      pn(2) =  center1(2);

    elseif ( center1(1) <= point(1,j) )

      pn(1) =  center1(1) + r1;
      pn(2) =  center1(2);

    end

    dist = sqrt ( ( point(1,j) - pn(1) ).^2 + ( point(2,j) - pn(2) ).^2 );

    if ( dist < dist_min )
      dist_min = dist;
      boundary(1:2,j) = pn(1:2)';
    end
%
%  Distance to semicircle #2.
%
    if ( center2(2) <= point(2,j) )

      norm = sqrt ( ( point(1,j) - center2(1) ).^2 ...
                  + ( point(2,j) - center2(2) ).^2 );

      if ( 0.0 == norm )

        pn(1) = center2(1) + r2 * sqrt ( 0.5 );
        pn(2) = center2(2) + r2 * sqrt ( 0.5 );

      else

        pn(1) = center2(1) + ( point(1,j) - center2(1) ) / norm;
        pn(2) = center2(2) + ( point(2,j) - center2(2) ) / norm;

      end

    elseif ( point(1,j) <= center2(1) )

      pn(1) =  center2(1) - r2;
      pn(2) =  center2(2);

    elseif ( center2(1) <= point(1,j) )

      pn(1) =  center2(1) + r2;
      pn(2) =  center2(2);

    end

    dist = sqrt ( ( point(1,j) - pn(1) ).^2 + ( point(2,j) - pn(2) ).^2 );

    if ( dist < dist_min )
      dist_min = dist;
      boundary(1:2,j) = pn(1:2)';
    end
%
%  Distance to line segment #1: (P1,P2).
%
    p1(1:2) = [ center2(1) - r1, center2(1) ];
    p2(1:2) = [ center2(1) - r2, center2(2) ];

    [ pn, dist, t ] = segment_point_near_2d ( p1, p2, point(1:2,j)' );

    if ( dist < dist_min )
      dist_min = dist;
      boundary(1:2,j) = pn(1:2)';
    end
%
%  Distance to line segment #2: (Q1,Q2).
%
    q1(1:2) = [ center2(1) + r2, center2(2) ];
    q2(1:2) = [ center1(1) + r1, center1(2) ];

    [ pn, dist, t ] = segment_point_near_2d ( q1, q2, point(1:2,j)' );

    if ( dist < dist_min )
      dist_min = dist;
      boundary(1:2,j) = pn(1:2)';
    end

  end

  return
end


function inside = polygon_contains_point_2d ( n, v, p )

%*****************************************************************************80
%
%% POLYGON_CONTAINS_POINT_2D finds if a point is inside a simple polygon in 2D.
%
%  Discussion:
%
%    A simple polygon is one whose boundary never crosses itself.
%    The polygon does not need to be convex.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    M Shimrat,
%    Position of Point Relative to Polygon,
%    ACM Algorithm 112,
%    Communications of the ACM,
%    Volume 5, Number 8, page 434, August 1962.
%
%  Parameters:
%
%    Input, integer N, the number of nodes or vertices in the polygon.
%    N must be at least 3.
%
%    Input, real V(2,N), the coordinates of the vertices of the polygon.
%
%    Input, real P(2,1), the coordinates of the point to be tested.
%
%    Output, logical INSIDE, is TRUE if the point is inside the polygon.
%
  inside = 0;

  for i = 1 : n

    x1 = v(1,i);
    y1 = v(2,i);

    if ( i < n )
      x2 = v(1,i+1);
      y2 = v(2,i+1);
    else
      x2 = v(1,1);
      y2 = v(2,1);
    end

    if ( ( y1 < p(2,1) & p(2,1) <= y2 ) | ( p(2,1) <= y1 & y2 < p(2,1) ) )
      if ( ( p(1,1) - x1 ) - ( p(2,1) - y1 ) * ( x2 - x1 ) / ( y2 - y1 ) < 0.0 )
        inside = ~inside;
      end
    end

  end

  return
end


function coef = sgmga_vcn_coef_naive ( dim_num, level_weight, x_max, x, q_max )

%*****************************************************************************80
%
%% SGMGA_VCN_COEF_NAIVE returns the "next" constrained vector's coefficient.
%
%  Discussion:
%
%    This function uses a naive approach to the computation, resulting in
%    a set of 2^DIM_NUM tasks.  Hence it is not suitable for cases where
%    DIM_NUM is moderately large.  The function SGMGA_VCN_COEF carries out
%    a more complicated but more efficient algorithm for the same computation.
%
%    We are given a nonnegative vector X of dimension DIM_NUM which satisfies:
%
%      sum ( 1 <= I <= DIM_NUM ) LEVEL_WEIGHT(I) * X(I) <= Q_MAX
%
%    This routine computes the appropriate coefficient for X in the
%    anisotropic sparse grid scheme.
%
%    The coefficient is calculated as follows:
%
%      Let B be a binary vector of length N, and let ||B|| represent
%      the sum of the entries of B.
%
%      Coef = sum ( all B such that X+B satisfies constraints ) (-1)^||B||
%
%    Since X+0 satisfies the constraint, there is always at least one
%    summand.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 May 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    An Anisotropic Sparse Grid Stochastic Collocation Method for Partial
%    Differential Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2411-2442.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the vector.
%
%    Input, real LEVEL_WEIGHT(DIM_NUM), the anisotropic weights.
%
%    Input, integer X_MAX(DIM_NUM), the maximum
%    values allowed in each component.
%
%    Input, integer X(DIM_NUM), a point which satisifies the constraints.
%
%    Input, real Q_MAX, the upper limit on the sum.
%
%    Output, real COEF, the combinatorial coefficient.
%

%
%  Force column vectors.
%
  level_weight = level_weight ( : );
  x = x ( : );

  b = zeros ( dim_num, 1 );
  coef = 1.0;

  while ( 1 )
%
%  Generate the next binary perturbation.
%
    b = binary_vector_next ( dim_num, b );
    b_sum = sum ( b(1:dim_num) );
%
%  We're done if we've got back to 0.
%
    if ( b_sum == 0 )
      break
    end
%
%  Perturb the vector.
%
    x2(1:dim_num,1) = x(1:dim_num,1) + b(1:dim_num,1);
%
%  Does it satisfy the XMAX constraint?
%  (THIS CHECK IS SURPRISINGLY NECESSARY, PARTLY BECAUSE OF ZERO WEIGHT).
%
    if ( any ( x_max(1:dim_num) < x2(1:dim_num) ) )
      continue
    end
%
%  Does it satisfy the Q_MAX constraint?
%
    q = level_weight(1:dim_num)' * x2(1:dim_num);

    if ( q <= q_max )
      coef = coef + r8_mop ( b_sum );
    end

  end

  return
end


function [ w, f ] = jed_to_weekday ( jed )

%*****************************************************************************80
%
%% JED_TO_WEEKDAY computes the day of the week from a JED.
%
%  Discussion:
%
%    BC 4713/01/01 => JED = 0.0 was noon on a Monday.
%
%    jedmod = mod ( 0.0D+00, 7.0D+00 ) = 0.0D+00
%    j = mod ( nint ( 0 ), 7 ) = 0
%    f = ( 0.0D+00 + 0.5D+00 ) - real ( j ) = 0.5D+00
%    w = i4_wrap ( 0 + 2, 1, 7 ) = 2 = MONDAY
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 April 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999.
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer W, the day of the week of the date.
%    The days are numbered from Sunday through Saturday, 1 through 7.
%
%    Output, real F, the fractional part of the day.
%
  jedmod = mod ( jed, 7.0 );

  j = mod ( floor ( jedmod ), 7 );

  f = ( jedmod + 0.5 ) - j;

  w = i4_wrap ( j + 2, 1, 7 );

  return
end


function [trans, points] = registerPoints3dAffine(points, target, varargin)
% Fit 3D affine transform using iterative algorithm.
%
%   TRANS = registerPoints3dAffine(POINTS, TARGET)
%   Computes the affine transform that maps the shape defines by POINTS
%   onto the shape defined by the points TARGET. Both POINTS and TARGET are
%   N-by-3 array of point coordinates, not necessarily the same size.
%   The result TRANS is a 4-by-4 affine transform.
%
%   TRANS = registerPoints3dAffine(POINTS, TARGET, NITER)
%   Specifies the number of iterations for the algorithm.
%
%   [TRANS, POINTS2] = registerPoints3dAffine(...)
%   Also returns the set of transformed points.
%
%   Example
%     registerPoints3dAffine
%
%   See also
%     transforms3d, fitAffineTransform3d
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2015-02-24,    using Matlab 8.4.0.150421 (R2014b)
% Copyright 2015 INRA - Cepia Software Platform.

nIters = 10;
if ~isempty(varargin)
    nIters = varargin{1};
end

% keep original points to transform them at each
trans = [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];

for i = 1:nIters
    % identify target points for each source point
    inds = findClosestPoint(points, target);
    corrPoints = target(inds, :);

    % compute transform for current iteration
    trans_i = fitAffineTransform3d(points, corrPoints);

    % apply transform, and update cumulated transform
    points = transformPoint3d(points, trans_i);
    trans = trans_i * trans;
end


% Question No: 7

% Given an image. Implement the split and merge procedure for segmenting
% the image with different values for minimum dimensions of the quadtree
% regions.

function quadtree(x)
f=imread(x);
q=2^nextpow2(max(size(f)));
[m n]=size(f);
f=padarray(f,[q-m,q-n],'post');
mindim=2;
s=qtdecomp(f,@split,mindim,@predicate);
lmax=full(max(s(:)));
g=zeros(size(f));
marker=zeros(size(f));
for k=1:lmax
    [vals,r,c]=qtgetblk(f,s,k);
    if ~isempty(vals)
        for i=1:length(r)
            xlow=r(i);ylow=c(i);
            xhigh=xlow+k-1;
            yhigh=ylow+k-1;
            region=f(xlow:xhigh,ylow:yhigh);
            flag=feval(@predicate,region);
            if flag
                 g(xlow:xhigh,ylow:yhigh)=1;
                 marker(xlow,ylow)=1;
            end
        end
    end
end
g=bwlabel(imreconstruct(marker,g));
g=g(1:m,1:n);
f=f(1:m,1:n);
figure, imshow(f),title('Original Image');
figure, imshow(g),title('Segmented Image');
end

function v=split(b,mindim,fun)
k=size(b,3);
v(1:k)=false;
for i=1:k
    quadrgn=b(:,:,i);
    if size(quadrgn,1)<=mindim
        v(i)=false;
        continue;
    end
    flag=feval(fun,quadrgn);
    if flag
        v(i)=true;
    end
end
end

function flag=predicate(region)
sd=std2(region);
m=mean2(region);
flag=(sd>5)&(m>0)&(m<200);
end


function [R,t,E] = psth(data,sig,plt,T,err,t)
%  function to plot trial averaged rate smoothed by
%  a Gaussian kernel - visual check on stationarity
%  Usage: [R,t,E] = psth(data,sig,plt,T,err,t)
%
%  Inputs:
% Note that all times have to be consistent. If data
% is in seconds, so must be sig and t. If data is in
% samples, so must sig and t. The default is seconds.
% data            structural array of spike times
% sig             std dev of Gaussian (default 50ms)
%                 (minus indicates adaptive with
%                  approx width equal to mod sig)
% plt = 'n'|'r' etc      (default 'r')
% T is the time interval (default all)
% err - 0 = none
%       1 = Poisson
%       2 = Bootstrap over trials (default)
% (both are based on 2* std err rather than 95%)
% t   = times to evaluate psth at
%
% The adaptive estimate works by first estimating
% the psth with a fixed kernel width (-sig) it
% then alters the kernel width so that the number
% of spikes falling under the kernel is the same on
% average but is time dependent.  Reagions rich
% in data therefore have their kernel width reduced
%
% Outputs:
%
% R = rate
% t = times
% E = errors (standard error)

if nargin <1 ; error('I need data!');end
[data]=padNaN(data); % create a zero padded data matrix from input structural array
sz=size(data);
% transposes data so that the longer dimension is assumed to be time
% Procedure used to bring input data into form compatible with that required
% for Murray's routine
if sz(1)>sz(2); data=data';end;
if nargin < 2; sig = 0.05;end
if nargin < 3; plt = 'r';end
if nargin < 5; err = 2; end

if isempty(sig); sig = 0.05;end
if isempty(plt); plt = 'r';end
if isempty(err); err = 2; end

adapt = 0;
if sig < 0; adapt = 1; sig = -sig; end

%  to avoid end effects increase time interval by the width
%  of the kernel (otherwise rate is too low near edges)

if nargin < 4;
  T(1) = min(data(:,1));
  T(2) = max(max(data));
else
  T(1) = T(1)-4*sig;
  T(2) = T(2)+4*sig;
end

% calculate NT and ND and filter out times of interest

NT = length(data(:,1));
if NT < 4 && err == 2
  disp('Using Poisson errorbars as number of trials is too small for bootstrap')
  err = 1;
end

m = 1;
D = zeros(size(data));
ND=zeros(1,NT);
for n=1:NT
  indx = find(~isnan(data(n,:)) & data(n,:)>=T(1) & data(n,:)<=T(2));
  ND(n) = length(indx);
  D(n,1:ND(n)) = data(n,indx);
  m = m + ND(n);
end
N_tot = m;
N_max = max(ND);
D = D(:,1:N_max);

% if the kernel density is such that there are on average
% one or less spikes under the kernel then the units are probably wrong

L = N_tot/(NT*(T(2)-T(1)));
if 2*L*NT*sig < 1 || L < 0.1
  disp('Spikes very low density: are the units right? is the kernel width sensible?')
  disp(['Total events: ' num2str(fix(100*N_tot)/100) ' sig: ' ...
        num2str(fix(1000*sig)) 'ms T: ' num2str(fix(100*T)/100) ' events/sig: ' ...
        num2str(fix(100*N_tot*sig/(T(2)-T(1)))/100)])
end

%    Smear each spike out
%    std dev is sqrt(rate*(integral over kernal^2)/trials)
%    for Gaussian integral over Kernal^2 is 1/(2*sig*srqt(pi))

if nargin < 6
  N_pts =  fix(5*(T(2)-T(1))/sig);
  t = linspace(T(1),T(2),N_pts);
else
  N_pts = length(t);
end

RR = zeros(NT,N_pts);
f = 1/(2*sig^2);
for n=1:NT
  for m=1:ND(n)
    RR(n,:) = RR(n,:) + exp(-f*(t-D(n,m)).^2);
  end
end
RR = RR*(1/sqrt(2*pi*sig^2));
if NT > 1; R = mean(RR); else R = RR;end

if err == 1
  E = sqrt(R/(2*NT*sig*sqrt(pi)));
elseif err == 2
  Nboot = 10;
  mE = 0;
  sE = 0;
  for b=1:Nboot
    indx = floor(NT*rand(1,NT)) + 1;
    mtmp = mean(RR(indx,:));
    mE = mE + mtmp;
    sE = sE + mtmp.^2;
  end
  E = sqrt((sE/Nboot - mE.^2/Nboot^2));
end

% if adaptive warp sig so that on average the number of spikes
% under the kernel is the same but regions where there is
% more data have a smaller kernel

if adapt
  sigt = mean(R)*sig./R;
  RR = zeros(NT,N_pts);
  f = 1./(2*sigt.^2);
  for n=1:NT
    for m=1:ND(n)
      RR(n,:) = RR(n,:) + exp(-f.*(t-D(n,m)).^2);
    end
    RR(n,:) = RR(n,:).*(1./sqrt(2*pi*sigt.^2));
  end
  if NT > 1; R = mean(RR); else R = RR;end

  if err == 1
    E = sqrt(R./(2*NT*sigt*sqrt(pi)));
  elseif err == 2
    Nboot = 10;
    mE = 0;
    sE = 0;
    for b=1:Nboot
      indx = floor(NT*rand(1,NT)) + 1;
      mtmp = mean(RR(indx,:));
      mE = mE + mtmp;
      sE = sE + mtmp.^2;
    end
    E = sqrt((sE/Nboot - mE.^2/Nboot^2));
  end
end

if plt == 'n';return;end
plot(t,R,plt)
hold on
if err > 0
  plot(t,R+2*E,'g')
  plot(t,R-2*E,'g')
end
%axis([T(1)+(4*sig) T(2)-(4*sig) 0 1.25*max(R)])
axis([T(1)+(4*sig) T(2)-(4*sig) 0 max(R+2*E)+10])
xlabel('time (s)')
ylabel('rate (Hz)')
title(['Trial averaged rate : Gaussian Kernel :'  ...
	    ' sigma = ' num2str(1000*sig) 'ms'])
hold off


function [u1, u2, u3, u4] = controller(current_state, desired_state, model_param, K)

J = model_param.I; % Moment of inertia wrt body frame

% position error
ep = current_state(1:3)-desired_state.pos;

% velocity error
ev = current_state(4:6)-desired_state.vel;

% desired force, F_des
Kp = K.Kp;
Kv = K.Kv;
KR = K.KR;
K_omega = K.K_omega;
m = model_param.mass;
g = model_param.grav;
Fd = -Kp.*ep -Kv.*ev + m*g*[0 0 1]' + m*desired_state.acc;

% desired input u1
yaw = current_state(9);
roll = current_state(7);
pitch = current_state(8);
R = ROTZ(yaw)*ROTX(roll)*ROTY(pitch); % Current rotation
% current z-axis in body frame
zb = R(:,3);
u1 = Fd'*zb;

current_acc = -g*[0 0 1]'+u1*zb;

% accel error
ea = current_acc-desired_state.acc;
% Desired force derivative
Fd_dot = -Kp*ev -Kv*ea + m*desired_state.jerk;
% desired rotation, Rd = [xbd ybd zbd]
zbd = Fd/norm(Fd);

% ybd = zbd X xcd/norm(zbd X xcd)
% xcd
yawd = desired_state.yaw;
yawd_dot = desired_state.yawdot;
yawd_2dot = desired_state.yawddot;

xcd = [cos(yawd) sin(yawd) 0]';
ybd = hat_optr(zbd)*xcd/norm(hat_optr(zbd)*xcd);
% xbd = ybd X zbd
xbd = hat_optr(ybd)*zbd;
Rd1 = [xbd ybd zbd];
Rd2 = [-xbd -ybd zbd];

% eR orientation error
R1 = (Rd1'*R - R'*Rd1);
R2 = (Rd2'*R - R'*Rd2);

eR1 = 1/2*vee_optr(R1);
eR2 = 1/2*vee_optr(R2);
% if norm(eR1) >= norm(eR2)
%     Rd = Rd2;
%     eR = eR2;
%     xbd = -xbd;
%     ybd = -ybd;
% else
    Rd = Rd1;
    eR = eR1;
% end

current_omega = current_state(10:12);

% Desired omega^ = Rd'*R
Fd_norm_dot = Fd'*Fd_dot/norm(Fd);
zbd_dot = (Fd_dot*norm(Fd) - Fd*Fd_norm_dot)/norm(Fd)^2;
xcd_dot = [-sin(yawd) cos(yawd) 0]'*desired_state.yawdot;

zbd_x_xcd_dot = hat_optr(zbd_dot)*xcd + hat_optr(zbd)*xcd_dot;
zbd_xcd_norm_dot = (hat_optr(zbd)*xcd)'*(zbd_x_xcd_dot)/norm(hat_optr(zbd)*xcd);

ybd_dot = (zbd_x_xcd_dot*norm(hat_optr(zbd)*xcd) - hat_optr(zbd)*xcd*zbd_xcd_norm_dot)/norm(hat_optr(zbd)*xcd)^2;
xbd_dot = hat_optr(ybd_dot)*zbd + hat_optr(ybd)*zbd_dot;

Rd_dot = [xbd_dot ybd_dot zbd_dot];
wd_hat = Rd'*Rd_dot;
wd = vee_optr(wd_hat);
% error
ew = current_omega - R'*Rd*wd;

% Desired angular accerelation^ = Rd_dot'*Rd_dot + Rd'*Rd_2dot;
R_dot = R*hat_optr(current_omega);
zb_dot = R_dot(:,3);
u1_dot = Fd_dot'*zb + Fd'*zb_dot;
current_jerk = (u1_dot*zb + u1*zb_dot)/m;
ej = current_jerk - desired_state.jerk;
Fd_2dot = -Kp*ea -Kv*ej + m*desired_state.snap;
zbd_2dot = (Fd_2dot*norm(Fd)-Fd_dot*Fd_norm_dot)/norm(Fd)^2 -...
           (((Fd_dot'*Fd_dot + Fd'*Fd_2dot)*Fd + (Fd'*Fd_dot)*Fd_dot)*norm(Fd)^3 -...
           (Fd'*Fd_dot)*Fd * 3*norm(Fd)*(Fd'*Fd_dot))/norm(Fd)^4;

% ybd_2dot
xcd_2dot = [-cos(yawd)*yawd_dot^2-sin(yawd)*yawd_2dot, -sin(yawd)*yawd_dot^2+cos(yawd)*yawd_2dot, 0]';

ybd_2dot_1 = ((hat_optr(zbd_2dot)*xcd + hat_optr(zbd)*xcd_2dot)*norm(hat_optr(zbd)*xcd)-...
             zbd_x_xcd_dot*zbd_xcd_norm_dot)/norm(hat_optr(zbd)*xcd)^2;

zbd_x_xcd_2dot = hat_optr(zbd_2dot)*xcd + 2*hat_optr(zbd_dot)*xcd_dot + hat_optr(zbd)*xcd_2dot;
zbd_xcd_norm_2dot = ((zbd_x_xcd_dot'*zbd_x_xcd_dot + (hat_optr(zbd)*xcd)'*zbd_x_xcd_2dot)*norm(hat_optr(zbd)*xcd)-...
                    ((hat_optr(zbd)*xcd)'*zbd_x_xcd_dot)*zbd_xcd_norm_dot)/norm(hat_optr(zbd)*xcd)^2;

ybd_2dot_2 = ((zbd_x_xcd_dot*zbd_xcd_norm_dot + hat_optr(zbd)*xcd*zbd_xcd_norm_2dot)*norm(hat_optr(zbd)*xcd)^2-...
              2*hat_optr(zbd)*xcd*norm(hat_optr(zbd)*xcd)*zbd_xcd_norm_dot^2)/norm(hat_optr(zbd)*xcd)^4;

ybd_2dot = ybd_2dot_1 - ybd_2dot_2;

xbd_2dot = hat_optr(ybd_2dot)*zbd + 2*hat_optr(ybd_dot)*zbd_dot + hat_optr(ybd)*zbd_2dot;

Rd_2dot = [xbd_2dot ybd_2dot zbd_2dot];
wd_dot_hat = Rd_dot'*Rd_dot + Rd'*Rd_2dot;
wd_dot = vee_optr(wd_dot_hat);

% Moment
M = -KR.*eR -K_omega.*ew + hat_optr(current_omega)*J*current_omega -J*(hat_optr(current_omega)*R'*Rd*wd - R'*Rd*wd_dot);

u2 = M(1);u3 = M(2); u4 = M(3);


%% Angular correlation between spherical harmonics profiles
%% Reference:
%%     Adam W. Anderson.
%%     Measurement of Fiber Orientation Distributions Using High Angular Resolution Diffusion Imaging.
%%     Magn. Reson. Med., 54(5):1194-1206, 2005.  (Eq.(71))
%% Example:
%%     [corr] = angular_correlation(coeff_u,coeff_v);
function [corr] = angular_correlation(coeff_u,coeff_v)
%%=============================================================
%% Project:   Spherical Harmonics
%% Module:    $RCSfile: angular_correlation.m,v $
%% Language:  MATLAB
%% Author:    $Author: bjian $
%% Date:      $Date: 2007/12/27 06:23:35 $
%% Version:   $Revision: 1.3 $
%%=============================================================

corr = (coeff_u'*coeff_v)/(norm(coeff_u)*norm(coeff_v));


%% function
clear; clc; close all;
T = 1000;
s = double(rand(1, T)>0.98);
sig = 0.04;

% example
tau_d = 5;
tau_r = 1;
nMax = 100;
pars = [tau_d, tau_r];
kernel = create_kernel('exp2', pars, nMax);

t = 1:kernel.nMax;
gt = kernel.fhandle(kernel.pars, t);
c1 = conv(s, gt);
c1 = c1(1:T);
y1 = c1 + randn(1, T) * sig;

%% use the true convolution kernel
kernel0 = kernel;
kernel0.pars = [10, 0.1];
kernel0.bound_pars = false;
figure('position', [1,1,1500, 200]);
plot(y1);
hold on;
plot(c1, 'r', 'linewidth', 2);
plot(-s*0.1, 'r', 'linewidth', 2);
tic;
[chat, shat, kernel_fit, iters] = deconvCa(y1, kernel0, 2, true, false);
toc;
plot(chat,'-.g','linewidth', 2);
plot(-shat*0.1, '-.g', 'linewidth', 2); %, '-.');
legend('data', 'ground truth: c','ground truth: s', 'OASIS:c', 'OASIS: s'); % 'gound truth: s', 'OASIS: c', 'OASIS: s');


function res = innerprod( x, y, dir, upto )
    %INNERPROD Inner product between two TT/MPS tensors.
    %   innerprod(X,Y) computes the inner product between the TT/MPS tensors X and Y.
    %   Assumes that the first rank of both tensors, X.rank(1) and Y.rank(1), is 1.
    %   The last rank may be different from 1, resulting in a matrix of size
    %   [X.rank(end), Y.rank(end)].
    %
    %   See also NORM

    %   TTeMPS Toolbox.
    %   Michael Steinlechner, 2013-2016
    %   Questions and contact: michael.steinlechner@epfl.ch
    %   BSD 2-clause license, see LICENSE.txt

    if ~exist( 'dir', 'var' )
        dir = 'LR';
    end
    if ~exist( 'upto', 'var' )
        if strcmpi( dir, 'LR')
            upto = x.order;
        else
            upto = 1;
        end
    end

    % Left-to-Right procedure
    if strcmpi( dir, 'LR')

        res = unfold( x.U{1}, 'left')' * unfold( y.U{1}, 'left');

        for i = 2:upto
            tmp = tensorprod_ttemps( x.U{i}, res', 1);
            res = unfold( tmp, 'left')' * unfold( y.U{i}, 'left');
        end

    % Right-to-Left procedure
    elseif strcmpi( dir, 'RL')
        d = x.order;
        res = conj(unfold( x.U{d}, 'right')) * unfold( y.U{d}, 'right').';

        for i = d-1:-1:upto
            tmp = tensorprod_ttemps( x.U{i}, res', 3);
            res = conj(unfold( tmp, 'right')) * unfold( y.U{i}, 'right').';
        end

    else
        error('Unknown direction specified. Choose either LR (default) or RL')
    end

end


function [forecastmatrix]=forecastsim(data_endo_a,data_exo_p,beta,n,p,k,horizon)

% function [forecastmatrix]=forecastsim(data_endo_a,data_exo_p,beta,n,p,k,horizon)
% computes the matrix of unconditional forecasts, using the chain rule of forecasts (see technical guide p38)
% inputs:  - matrix 'data_endo_a': matrix of pre-forecast endogenous data
%          - matrix 'data_exo_p': predicted values for the exogenous variables over the forecast periods
%          - vector 'beta': vectorised form of VAR coefficients (definined in 1.1.12)
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
%          - integer 'horizon': number of forecast periods
% outputs: - matrix 'forecast_matrix': matrix recording the forecast values

% uses the chain rule of forecasting, p38 of the technical guide

% compute the reduced matrix Y
Y=data_endo_a(end-p+1:end,:);

% reshape beta to obtain B
B=reshape(beta,k,n);

% repeat the process for periods T+1 to T+h
for jj=1:horizon

% step 1
% use the function lagx to obtain the matrix temp
temp=bear.lagx(Y,p-1);

% step 2
% define the reduced regressor matrix X
% if no exogenous variable is present at all in the model (neither constant nor other exogenous), define X only from the endogenous variables
if isempty(data_exo_p)==1
X=[temp(end,:)];
% if there are exogenous variables, concatenate them next to the endogenous
else
X=[temp(end,:) data_exo_p(jj,:)];
end

% step 3
% obtain the predicted value for T+jj
yp=X*B;

% step 4
% concatenate yp to the top of Y
Y=[Y;yp];

% repeat until values are obtained for T+h
end

% record the values in the matrix forecastmatrix
forecastmatrix=Y(p+1:end,:);


%  Figure 10.57      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% fig10_57.m is a script for disk read/write head control design
% the design is based on 1/s^2 with robustness
% with respect to a resonance of damping zeta = z at wm.
% the design calls for a single lead selected to maximize wc
% the phase margin is set by alpha = a
% the resonance is 'gain stabilized' with GM
% the time is scaled to milliseconds
clf;
wm= 5*pi
z=.05;
GM = 4;
a=0.1;
numGo=1;
denGo= [1 0 0];
sysGo=tf(numGo,denGo);
numG= [1/(50*pi) 1];
denG=[1/(25*pi^2) 1/(50*pi) 1  0 0];
sysG=tf(numG,denG);
b=GM/(2*z);
% PM >50; select alpha = 0.1
T= (1/wm)*sqrt(b/a^1.5);
wc = 1/(sqrt(a)*T)
% K = sqrt(a)*wc^2
K= sqrt(a)*wm^2/b;
numD =K*sqrt(a)*[T 1];
denD = [a*T 1];
sysD=tf(numD,denD);
sysOLo=sysD*sysGo;
sysOL=sysD*sysG;
figure(1)
clf
% bode(sysOLo);
% hold on
w=logspace(-1,2);
[mag,ph]=bode(sysOL,w);
subplot(211);
% convert to db
magdb=20*log10(mag(:,:));
magdb1=[magdb; zeros(size(magdb))];
semilogx(w,magdb1);
grid;
title('Fig. 10.57 Disk drive control with gain-stabilized resonance');
xlabel('\omega (rad/sec)');
ylabel('Magnitude (db)');
subplot(212);
semilogx(w,[ph(:,:); -180*ones(size(ph(:,:)))]);
grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
pause;
%hold off
[Gm,Pm,Wcg,Wcp] = margin(mag,ph,w)


function [ x, seed ] = anglit_sample ( seed )

%*****************************************************************************80
%
%% ANGLIT_SAMPLE samples the Anglit PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  [ cdf, seed ] = r8_uniform_01 ( seed );

  x = anglit_cdf_inv ( cdf );

  return
end


function poisson_simulation_test02 ( )

%*****************************************************************************80
%
%% POISSON_SIMULATION_TEST02 simulates waiting for a given length of time.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 September 2012
%
%  Author:
%
%    John Burkardt
%
  lambda = 0.5;
  t = 1000.0;
  test_num = 20000;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POISSON_SIMULATION_TEST02:\n' );
  fprintf ( 1, '  POISSON_FIXED_EVENTS simulates a Poisson process\n' );
  fprintf ( 1, '  counting the number of events that occur during\n' );
  fprintf ( 1, '  a given time.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Simulate a Poisson process, for which, on average,\n' );
  fprintf ( 1, '  LAMBDA events occur per unit time.\n' );
  fprintf ( 1, '  Run for a total of %g time units.\n', t );
  fprintf ( 1, '  LAMBDA = %g\n', lambda );

  n = zeros ( test_num, 1 );
  for test = 1 : test_num
    n(test) = poisson_fixed_time ( lambda, t );
  end

  n_mean = mean ( n );
  n_var = var ( n );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Mean number of events = %g\n', n_mean );
  fprintf ( 1, '  Variance = %g,  STD = %g\n', n_var, sqrt ( n_var ) );

  bins = 30;
  hist ( n, bins );
  xlabel ( '<--Number of events-->' )
  ylabel ( '<--Frequency-->' )
  grid on
  title ( ' Number of Poisson events observed over 1000 time units' );

  filename = 'poisson_events.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot file saved as "%s"\n', filename );

  return
end


function [col4row,row4col,gain]=assign2DHungarian(C,maximize)
%%ASSIGN2DHUNGARIAN Solve the two-dimensional assignment problem with a
%          rectangular cost matrix C using the O(n^4) complexity Hungarian
%          algorithm. The problem being solved can be formulated as
%          minimize (or maximize)
%          \sum_{i=1}^{numRow}\sum_{j=1}^{numCol}C_{i,j}*x_{i,j}
%          subject to
%          \sum_{j=1}^{numCol}x_{i,j}<=1 for all i
%          \sum_{i=1}^{numRow}x_{i,j}=1 for all j
%          x_{i,j}=0 or 1.
%          Assuming that numCol<=numRow. If numCol>numRow, then the
%          inequality and inequality conditions are switched. The function
%          assign2D uses an O(n^3) shortest augmenting path modification of
%          the Hungarian algorithm and thus should be preferred over this
%          algorithm.
%
%INPUTS: C A numRowXnumCol cost matrix that does not contain any NaNs and
%          where the largest finite element minus the smallest element is a
%          finite quantity (does not overflow) when performing minimization
%          and where the smallest finite element minus the largest
%          element is finite when performing maximization. Forbidden
%          assignments can be given costs of +Inf for minimization and -Inf
%          for maximization.
% maximize If true, the minimization problem is transformed into a
%          maximization problem. The default if this parameter is omitted
%          or an empty matrix is passed is false.
%
%OUTPUTS: col4row A numRowX1 vector where the entry in each element is an
%                 assignment of the element in that row to a column. 0
%                 entries signify unassigned rows. If the problem is
%                 infeasible, this is an empty matrix.
%         row4col A numColX1 vector where the entry in each element is an
%                 assignment of the element in that column to a row. 0
%                 entries signify unassigned columns. If the problem is
%                 infeasible, this is an empty matrix.
%            gain The sum of the values of the assigned elements in C. If
%                 the problem is infeasible, this is -4
%
%This function implements the O(n^4) complexity Hungarian algorithm, which
%is the combination of algorithms 4.2 and 4.3 of Chapter 4.2 of [1]. The
%algorithm has been slightly modified to handle rectangular matrices and to
%transform the input matrix to have all positice elements before running
%the algorithm, so that the algorithm can work on general matrices.
%Additionally, the algorithm contains a floating-point comparison to zero,
%which has been replaced by a threshold to make the algorithm more robust
%when handling non-integer costs.
%
%See the comments to assign2D for examples. The function assign2D
%
%REFERENCES:
%[1] R. Burkard, M. Dell'Amico, and S. Martello, Assignment Problems.
%    Philadelphia: Society for Industrial and Applied Mathematics, 2009.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(maximize))
    maximize=false;
end

numU=size(C,1);
numV=size(C,2);

didTranspose=false;
if(numU>numV)
    C=C';

    temp=numU;
    numU=numV;
    numV=temp;

    didTranspose=true;
end

%The cost matrix must have all non-negative elements for the assignment
%algorithm to work. This forces all of the elements to be positive. The
%delta is added back in when computing the gain in the end.
if(maximize==true)
    CDelta=max(C(:));

    %If C is all negative, do not shift.
    if(CDelta<0)
        CDelta=0;
    end

    C=-C+CDelta;
else
    CDelta=min(C(:));

    %If C is all positive, do not shift.
    if(CDelta>0)
        CDelta=0;
    end

    C=C-CDelta;
end

UScanned=zeros(numU,1);%Allocate space
VLabeledUnscanned=zeros(numV,1);%Allocate space
pred=zeros(numV,1);%Allocate space

VLabeled=zeros(numV,1);%Allocate space.

unassignedU=1:numU;
numUnassignedU=numU;
%Quick indicator of which vertex is assigned.
assignedBool=false(numU,1);
numAssignedU=0;

%Allocate and initialize.
u=zeros(numU,1);
v=zeros(numV,1);
row4col=zeros(numV,1);
col4row=zeros(numU,1);

while(numAssignedU<numU)
    kIdx=1;
    k=unassignedU(kIdx);
    while(assignedBool(k)==false)
        %%%GET THE ALTERNATING PATH

        %Unlabeled vertices.
        VUnlabeled=1:numV;
        numUnlabeled=numV;
        numLabeledUnscanned=0;
        numScannedU=0;
        numLabeled=0;

        fail=false;
        sink=0;
        i=k;
        while(fail==false&&sink==0)
            numScannedU=numScannedU+1;
            UScanned(numScannedU)=i;
            jIdx=1;
            while(jIdx<=numUnlabeled)
                j=VUnlabeled(jIdx);
                cost=C(i,j)-u(i)-v(j);
                %Ad-hoc threshold for testing if zero.
                if(cost<=4*eps(C(i,j)))
                    pred(j)=i;
                    %Remove from the list of unlabeled vertices.
                    VUnlabeled(jIdx)=VUnlabeled(numUnlabeled);
                    numUnlabeled=numUnlabeled-1;

                    %Add to the list of labeled but unscanned vertices.
                    numLabeledUnscanned=numLabeledUnscanned+1;
                    VLabeledUnscanned(numLabeledUnscanned)=j;

                    %Also just add to the list of labeled vertices.
                    numLabeled=numLabeled+1;
                    VLabeled(numLabeled)=j;
                else
                    jIdx=jIdx+1;
                end
            end
            if(numLabeledUnscanned==0)
                fail=true;
            else
                %Scan the last labeled, unscanned vertex.
                j=VLabeledUnscanned(numLabeledUnscanned);
                numLabeledUnscanned=numLabeledUnscanned-1;

                if(row4col(j)==0)
                    sink=j;
                else
                    i=row4col(j);
                end
            end
        end

        %%UPDATE USING THE ALTERNATING PATH
        if(sink>0)
            %Increase the primal solution.
            assignedBool(k)=true;
            numAssignedU=numAssignedU+1;
            unassignedU(kIdx)=unassignedU(numUnassignedU);
            numUnassignedU=numUnassignedU-1;

            j=sink;
            while(1)
                i=pred(j);
                row4col(j)=i;
                h=col4row(i);
                col4row(i)=j;
                j=h;
                if(i==k)
                    break;
                end
            end
        else
            %Update the dual solution.
            deltaVal=Inf;
            for curU=1:numScannedU
                i=UScanned(curU);
                for curV=1:numUnlabeled
                    j=VUnlabeled(curV);
                    cost=C(i,j)-u(i)-v(j);

                    if(cost<deltaVal)
                        deltaVal=cost;
                    end
                end
            end

            %If the problem is infeasible.
            if(~isfinite(deltaVal))
                col4row=[];
                row4col=[];
                gain=-1;
                return;
            end

            for curU=1:numScannedU
                i=UScanned(curU);
                u(i)=u(i)+deltaVal;
            end

            for curV=1:numLabeled
                j=VLabeled(curV);
                v(j)=v(j)-deltaVal;
            end
        end
    end
end

if(nargout>2)
    gain=0;
    for curRow=1:numU
        gain=gain+C(curRow,col4row(curRow));
    end

    %Adjust the gain for the initial offset of the cost matrix.
    if(maximize)
        gain=-gain+CDelta*numU;
    else
        gain=gain+CDelta*numU;
    end
end

if(didTranspose)
    temp=col4row;
    col4row=row4col;
    row4col=temp;
end
end

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


function C = VBA_spm_mesh_curvature(M)
% Compute a crude approximation of the curvature of a surface mesh
% FORMAT C = spm_mesh_curvature(M)
% M        - a patch structure
%
% C        - curvature vector
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Guillaume Flandin
% $Id: spm_mesh_curvature.m 3135 2009-05-19 14:49:42Z guillaume $

A = VBA_spm_mesh_adjacency(M);
A = sparse(1:size(M.vertices,1),1:size(M.vertices,1),1./sum(A,2)) * A;

C = (A-speye(size(A))) * double(M.vertices);
N = VBA_spm_mesh_normals(M);
C = sign(sum(N.*C,2)) .* sqrt(sum(C.*C,2));


function diff = hist_dist( hist1, hist2, method )
    switch method
        case 'x2'
            % diff = 0.5 * sum((hist1 - hist2).^2) ./ sum(hist1 + hist2 + eps);
            diff = 0.5 * sum( (hist1 - hist2).^2 ./ (hist1 + hist2 + eps) );
        case 'jsd'      % Jensen-Shannon Divergence
            diff = 0.5*(sum(hist1.*log((hist1+eps)./(hist2+eps))) + sum(hist2.*log((hist2+eps)./(hist1+eps))));
        otherwise
            error( 'unknown type for computing histogram distance' );
    end
end


% polar angle of closest point on the wall
function [data,units] = compute_arena_angle(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);

  data{i} = atan2(trx(fly).y_mm-trx.landmark_params{n}.arena_center_mm_y(n),...
    trx(fly).x_mm-trx.landmark_params{n}.arena_center_mm_x(n));

end
units = parseunits('rad');


% coordinates of the three rings (blue, red and green)
[X1,Y1,Z1]=torus(5-0.3,30,0.3,2); % the top (blue)
[X2,Z2,Y2]=torus(5+0.3,30,0.3,2); % inner gimbal (green, semi-transparent)
[X3,Z3,Y3]=torus(5+0.9,30,0.3,1); % outer gimbal (red, semi-transparent)

% handles to surf plots of the three rings
H1=surf(X1,Y1,Z1,'EdgeColor','none','FaceColor','blue'); hold on;
H2=surf(X2,Y2,Z2,'EdgeColor','none','FaceColor','green','FaceAlpha',0.3);
H3=surf(X3,Y3,Z3,'EdgeColor','none','FaceColor','red','FaceAlpha',0.3);

% creates the bars inside the inner (blue) ring. The wide cylinders
% represent weights.
[Xbar1,Ybar1,Zbar1,Xbar2,Ybar2,Zbar2]=createInnerBars(0.2,0.5,0.2,0.5);
bar1=surf(Xbar1,Ybar1,Zbar1,'EdgeColor','none','FaceColor','blue');
bar2=surf(Xbar2,Ybar2,Zbar2,'EdgeColor','none','FaceColor','blue');

% the red motionless supporting bar
[ Xupper,Yupper,Zupper,Xlower,Ylower,Zlower ] = createOuterBars( 0.3 );
surf(Xlower,Ylower,Zlower,...
    'EdgeColor','none','FaceColor','red','FaceAlpha',0.3);

hold off;
 camlight left;
 lighting gouraud; % phong is more demanding but gives nicer results
 bb=7;
 axis([-bb bb -bb bb -bb bb]);
axis square;
xlabel('x'); ylabel('y'); zlabel('z');


function test_Script()

% GEOMETRIC PARAMETERS
pi = 4*atan(1);
L1 = 8;                              % length of computational domain (m)
N1 = 512;                            % number of Cartesian grid meshwidths at the finest level of the AMR grid
bell_length = 2;                     % bell length (m)
bell_circumference = pi;
npts_bell = ceil(2*(bell_length/L1)*N1);  % number of pos along the length of the bell
npts_circ = 1; %number of pos along the circumference (if in 3D)
npts = npts_bell*npts_circ;	    % total number pos
ds1 = bell_length/(npts_bell-1);   % mesh spacing(m) along length of bell

% Values from Alben, Peng, and Miller
betao = 0.5;
betam = 0.3;
to = 0.4;
Zs = L1/8;
xRef = L1/2;

t=0:0.025:5;

for i=1:length(t)

    %These are used to keep track of cycle number and time into the cycle
    pulse_time = t(i)-floor(t(i)); % determine time since beginning of first pulse

    % GIVE BELL STATES
    if (pulse_time<to)  %contract bell
        beta = betao+(betam-betao)*(pulse_time/to);
    else                % expand bell
        beta = betam+(betao-betam)*((pulse_time-to)/(1-to));
    end

    %---------------------
    %Xb and Yb are calculated here and will be used to determine new curvatures
    s1=0;
    zl = Zs;
    ro = xRef;

    % Pre-allocate memory for speed
    Xb_lam = zeros(npts,1);
    Yb_lam = zeros(npts,1);

    %top of bell
    Xb_lam(1)=ro;
    Yb_lam(1)=zl;

    %right side of bell
    for s1 = 2:ceil(npts_bell/2)
        thetaj = -1.55*(1-exp(-(s1)*ds1/beta));
        zl = zl + ds1*sin(thetaj);
        ro = ro + ds1*cos(thetaj);
        Xb_lam(s1)=ro;
        Yb_lam(s1)=zl;
    end

    zl = Zs;
    ro = xRef;

    %left side of bell
    for s1 = (ceil(npts_bell/2))+1:npts_bell
        s2=s1-(ceil(npts_bell/2));
        thetaj = -1.55*(1-exp(-(s2)*ds1/beta));
        zl = zl + ds1*sin(thetaj);
        ro = ro - ds1*cos(thetaj);
        Xb_lam(s1)=ro;
        Yb_lam(s1)=zl;
    end

    plot(Xb_lam,Yb_lam,'.'); hold on;
    axis([0 8 0 8]);
    pause(0.001);
    clf;

end


function [outputCONTOUR,varargout] = TRACE_MooreNeighbourhood(data2D,varargin)
% TRACE_MooreNeighbourhood  2D boundary tracing using the Moore neighbourhood
%==========================================================================
% FILENAME:     TRACE_MooreNeighbourhood.m
% AUTHOR:       Adam H. Aitkenhead
% DATE:         12th April 2010
% PURPOSE:      2D boundary tracing using the Moore neighbourhood
%
% USAGE:        [listCONTOUR,listNORMALS] = TRACE_MooreNeighbourhood(data2D,pixelFIRST)
%%
% INPUT PARAMETERS:  data2D      - Mandatory.  An LxM array containing the
%                                  2D data.  All pixels within the region
%                                  to be traced must equal 1, and all
%                                  pixels outside the region must equal 0.
%                    pixelFIRST  - Optional.  A (2x1) array containing the
%                                  [x;y] coordinates of any one pixel
%                                  located on the edge of the region to be
%                                  traced.
%
% OUTPUT PARAMETERS: listCONTOUR - Mandatory.  An Nx2 array containing the
%                                  coordinates of the pixels located around
%                                  the edge of the region to be traced.
%                                  The pixels are listed in a clockwise
%                                  direction around the contour.
%                    listNORMALS - Optional.  An Nx2 array containing the
%                                  (approximate) normal vectors pointing
%                                  out of the region of interest.  The
%                                  vectors are listed in a clockwise
%                                  direction around the contour, and
%                                  correspond to each pixel listed in
%                                  listCONTOUR.
%
% EXAMPLE:      data2D = [0,0,0,0,0,0; 0,1,0,1,0,0; 0,1,1,1,1,0; 0,1,1,1,0,0; 0,1,1,1,1,0; 0,0,0,0,0,0];
%               [listCONTOUR,listNORMALS] = TRACE_MooreNeighbourhood(data2D);
%               imagesc(data2D)
%               colormap('gray')
%               hold on
%                 plot(listCONTOUR(:,2),listCONTOUR(:,1),'ro-','LineWidth',2)
%                 plot([listCONTOUR(:,2),listCONTOUR(:,2)+listNORMALS(:,2)]',[listCONTOUR(:,1),listCONTOUR(:,1)+listNORMALS(:,1)]','b','LineWidth',2)
%               hold off
%
% NOTES:      - The algorithm ends when the *second* pixel in the loop is
%               revisited, entered from the same direction as it was
%               entered on its first visit.  The second pixel is used
%               rather than the first pixel for two reasons:  1. The
%               first pixel may be encountered more than once while going
%               around the contour;  2. It is possible that the first pixel
%               will never be entered from the same direction as it was on
%               its first visit.
%             - The code does not identify any internal holes which exist
%               in the region to be traced.
%             - If more than one separate region exists in the image, the
%               code only traces one of these regions.  The user can define
%               which region by choosing an appropriate starting pixel
%               <pixelFIRST>.  Alternatively, the first region encountered
%               by the algorithm will be traced.
%
% REFERENCES: - For a guide to Contour Tracing, including a description of
%               Moore-Neighbour Tracing, see the tutorial by Abeer George
%               Ghuneim at:
%                http://www.imageprocessingplace.com/downloads_V3/
%                  root_downloads/tutorials/
%                  contour_tracing_Abeer_George_Ghuneim/index.html
%==========================================================================

%==========================================================================
% VERSION:  USER:  CHANGES:
% -------   -----  --------
% 090512    AHA    Original version
% 091021    AHA    Improved the calculation of the normals
% 100412    AHA    General housekeeping
%==========================================================================

%================================================
%  READ INPUT PARAMETERS
%================================================

% Configure the array which contains the region to be contour-traced:
if nargin<1
  error(' ERROR:  A 2D array is required as input.');
end

% Define pixelFIRST, the start coordinate (x,y) for the Moore-neighbour tracing algorithm.  This must be an edge pixel on the region to be contour-traced.
if nargin==2
  pixelFIRST = varargin{1};
else %If pixelFIRST hasn't been defined by the user, define it now:
  [tempR,tempC] = find(data2D==1);
  pixelFIRST = [tempR(1);tempC(1)];
end

%Make sure pixelFIRST is a column vector, rather than a row vector
if size(pixelFIRST,2)==2
  pixelFIRST = pixelFIRST';
end

%Makes sure data2D is a binary image
if numel(unique(data2D))~=2  ||  min(data2D(:))~=0  ||  max(data2D(:))~=1
  error(' Input image must be an array composed only of 0s and 1s.')
end

% Add a one-pixel border around the array before doing the analysis.
% This prevents the code from failing if the region to be traced extends to the edge of the array.
data2Doriginal = data2D;
data2D         = [zeros(1,size(data2Doriginal,2)+2);zeros(size(data2Doriginal,1),1),data2Doriginal,zeros(size(data2Doriginal,1),1);zeros(1,size(data2Doriginal,2)+2)];
pixelFIRST     = pixelFIRST + 1;

%================================================
%  INITIAL DEFINITIONS
%================================================

%Define the initial direction of entry (x,y) into the first pixel.  This
%direction must point from the outside to the inside.
tempI = find(data2D(pixelFIRST(1)-1:pixelFIRST(1)+1,pixelFIRST(2))==0);
if isempty(tempI)==0
  directionFIRST = [2-tempI(1);0];
else
  tempI = find(data2D(pixelFIRST(1),pixelFIRST(2)-1:pixelFIRST(2)+1)==0);
  if isempty(tempI)==0
    directionFIRST = [0;2-tempI(1)];
  else
    imagesc(data2D)
    save('temp.txt','data2D','-ascii','-tabs')
    error(' ERROR:  Unable to identify a vector pointing into the first pixel on the contour.');
  end
end

%Create an array for the output.  It will hold the (x,y) coordinates of
%every pixel around the border of the region to be contour-traced.  The
%directions are also recorded, and will be used as part of the test
%criterion to end the algorithm:
listCONTOUR   = pixelFIRST;
listDIRECTION = directionFIRST;

%Create a variable which will be used to tell the algorithm when to stop:
checkLOOPEND = 0;

%================================================
%  PERFORM THE MOORE NEIGHBOURHOOD TRACING
%================================================

ROTangle     = -pi/2;   %radians
ROTmatrix90  = [ cos(ROTangle) , -sin(ROTangle) ; sin(ROTangle) , cos(ROTangle) ];
ROTangle     = -pi;   %radians
ROTmatrix180 = [ cos(ROTangle) , -sin(ROTangle) ; sin(ROTangle) , cos(ROTangle) ];

tempPIX = pixelFIRST;
tempDIR = directionFIRST;

while checkLOOPEND==0

  tempDIR = round(ROTmatrix180*tempDIR);
  tempPIX = tempPIX + tempDIR;
  if data2D(tempPIX(1),tempPIX(2)) == 1
    listCONTOUR   = [listCONTOUR,tempPIX];
    listDIRECTION = [listDIRECTION,tempDIR];
  else
    tempDIR = round(ROTmatrix90*tempDIR);
    tempPIX = tempPIX + tempDIR;
    if data2D(tempPIX(1),tempPIX(2)) == 1
      listCONTOUR   = [listCONTOUR,tempPIX];
      listDIRECTION = [listDIRECTION,tempDIR];
    else
      tempDIR = round(ROTmatrix90*tempDIR);
      tempPIX = tempPIX + tempDIR;
      if data2D(tempPIX(1),tempPIX(2)) == 1
        listCONTOUR   = [listCONTOUR,tempPIX];
        listDIRECTION = [listDIRECTION,tempDIR];
      else
        tempPIX = tempPIX + tempDIR;
        if data2D(tempPIX(1),tempPIX(2)) == 1
          listCONTOUR   = [listCONTOUR,tempPIX];
          listDIRECTION = [listDIRECTION,tempDIR];
        else
          tempDIR = round(ROTmatrix90*tempDIR);
          tempPIX = tempPIX + tempDIR;
          if data2D(tempPIX(1),tempPIX(2)) == 1
            listCONTOUR   = [listCONTOUR,tempPIX];
            listDIRECTION = [listDIRECTION,tempDIR];
          else
            tempPIX = tempPIX + tempDIR;
            if data2D(tempPIX(1),tempPIX(2)) == 1
              listCONTOUR   = [listCONTOUR,tempPIX];
              listDIRECTION = [listDIRECTION,tempDIR];
            else
              tempDIR = round(ROTmatrix90*tempDIR);
              tempPIX = tempPIX + tempDIR;
              if data2D(tempPIX(1),tempPIX(2)) == 1
                listCONTOUR   = [listCONTOUR,tempPIX];
                listDIRECTION = [listDIRECTION,tempDIR];
              else
                tempPIX = tempPIX + tempDIR;
                if data2D(tempPIX(1),tempPIX(2)) == 1
                  listCONTOUR   = [listCONTOUR,tempPIX];
                  listDIRECTION = [listDIRECTION,tempDIR];
                else
                  tempDIR = round(ROTmatrix90*tempDIR);
                  tempPIX = tempPIX + tempDIR;
                  if data2D(tempPIX(1),tempPIX(2)) == 1
                    listCONTOUR   = [listCONTOUR,tempPIX];
                    listDIRECTION = [listDIRECTION,tempDIR];
                  else
                    tempDIR = round(ROTmatrix90*tempDIR);
                    tempPIX = tempPIX + tempDIR;
                    if data2D(tempPIX(1),tempPIX(2)) == 1
                      listCONTOUR   = [listCONTOUR,tempPIX];
                      listDIRECTION = [listDIRECTION,tempDIR];
                    end %if

                  end %if
                end %if
              end %if
            end %if
          end %if
        end %if
      end %if
    end %if
  end %if

  if size(listCONTOUR,2)>2  &&  min(isequal(tempPIX,listCONTOUR(:,2)))  &&  min(isequal(tempDIR,listDIRECTION(:,2)))
    %The stop criterion has been met, so tell the while-loop to exit:
    checkLOOPEND=1;
    %Remove the extra col from the results corresponding to the second
    %visit to the second pixel.
    listCONTOUR         = listCONTOUR(:,1:end-1);
    listDIRECTION       = listDIRECTION(:,1:end-1);
    listENTRYDIRECTIONS = -listDIRECTION;
    %Make sure the normal vector for the first pixel is the same on its
    %first visit as on its last vist:
    listENTRYDIRECTIONS(:,1) = listENTRYDIRECTIONS(:,end);
  end %if

end %while

% Adjust the results to remove the effect of the one-pixel border that was
% added around the array before the contour was traced:
listCONTOUR = listCONTOUR - 1;

%================================================
%  COMPUTE THE NORMALS
%  A combination of two methods is used to compute the normals:
%  -1-  The gradient of the two surrounding contour points is found, and
%       then rotated 90 degrees to get the gradient at the point of
%       interest.
%  -2-  The midpoint of the two surrounding points is found, and the vector
%       from this point to the point of interest is found.
%  Method 1 handles concave edges and diagonal edges better, but gets
%  confused in places where the region of interest ends in a one-pixel-wide
%  line, since at that point the two surrounding points are coincident.
%================================================

% Remove last point, which is a duplicate of the first point.
listCONTOURtemp = listCONTOUR(:,1:end-1);

% Method 1:  Gradient:
listGRADIENTS = (circshift(listCONTOURtemp,[0,-1])-circshift(listCONTOURtemp,[0,1]))/2;  % Calculate the gradient of the two surrounding contour-points.
ROTangle      = pi/2;   %radians                                                         % Rotate the gradient by +90 degree to produce the normals.
ROTmatrix     = [ cos(ROTangle) , -sin(ROTangle) ; sin(ROTangle) , cos(ROTangle) ];
listNORMALS_G = ROTmatrix * listGRADIENTS;

% Method 2:  Midpoint:
listMIDPOINT  = (circshift(listCONTOURtemp,[0,-1])+circshift(listCONTOURtemp,[0,1]))/2;  %Find the midpoint of the two surrounding contour-points.
listNORMALS_M = (listCONTOURtemp - listMIDPOINT)./2;                                     % Calculate the vector to the point of interest

% Combine results from the two methods:
method1sign = sign(round(10*listNORMALS_G));
method2sign = sign(round(10*listNORMALS_M));
method1sign(method1sign==0) = method2sign(method1sign==0);
listNORMALS_M = method1sign.*abs(listNORMALS_M);
listNORMALS        = listNORMALS_M;
listNORMALS(:,:,2) = listNORMALS_G;
listNORMALS = mean(listNORMALS,3);

% For cases where the ROI is larger than a single pixel, ensure the normals are units vectors
if size(listNORMALS,2)>1
  normalLENGTHS = sqrt(sum(listNORMALS.^2,1));
  listNORMALS   = listNORMALS./([1;1]*normalLENGTHS);
else
  listNORMALS = [-1;0];
end

% Replace last point, which is a duplicate of the first point.
listNORMALS   = [listNORMALS,listNORMALS(:,1)];

%================================================
%  DEFINE THE OUTPUT ARGUMENTS
%================================================

outputCONTOUR = listCONTOUR';
listNORMALS   = listNORMALS';

if nargout == 2
  varargout(1) = {listNORMALS};
end


function fem1d_spectral_numeric_test ( )

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_SPECTRAL_NUMERIC_TEST:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the FEM1D_SPECTRAL_NUMERIC library.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Run FEM1D_SPECTRAL_NUMERIC with an increasing number of basis functions.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   N        L2-error        H1-error\n' );
  fprintf ( 1, '\n' );

  for n = 1 : 2 : 11
    [ l2, h1 ] = fem1d_spectral_numeric ( n );
    fprintf ( 1, '  %2d  %14.6g  %14.6g\n', n, l2, h1 );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_SPECTRAL_NUMERIC_TEST:\n' );
  fprintf ( 1, '  Normal end of execution\n' );

  return
end


function timing_MX
% TIMING_MX  Speed of MX as performed by MULTIPROD and by a nested loop.
%    TIMING_MX compares the speed of matrix expansion as performed by
%    MULTIPROD and an equivalent nested loop. The results are shown in the
%    manual (fig. 2).
%    Notice that MULTIPROD enables array expansion which generalizes matrix
%    expansion to arrays of any size, while the loop tested in this
%    function works only for this specific case, and would be much slower
%    if it were generalized to N-D arrays.

% Checking whether needed software exists
message = sysrequirements_for_testing('timeit');
if message
    disp ' ', error('testing_memory_usage:Missing_subfuncs', message)
end

% Matrix expansion example (fig. 2)
disp ' '
disp 'Timing matrix expansion (see MULTIPROD manual, figure 2)'
disp ' '

a = rand(2, 5);
b = rand(5, 3, 1000, 10);

fprintf ('Size of A:  %0.0fx%0.0f\n', size(a))
fprintf ('Size of B: (%0.0fx%0.0f)x%0.0fx%0.0f\n', size(b))
disp ' ', disp 'Please wait...'
disp ' '

f1 = @() loop(a,b);
f2 = @() multiprod(a,b);

t1 = timeit(f1)*1000;
fprintf('LOOP(A, B):      %10.4f milliseconds\n', t1)
t2 = timeit(f2)*1000;
fprintf('MULTIPROD(A, B): %10.4f milliseconds\n', t2)

disp ' '
fprintf('MULTIPROD performed matrix expansion %6.0f times faster than a plain loop\n', t1/t2)
disp ' '

function C = loop(A,B)
for i = 1:1000
    for j = 1:10
        C(:,:,i,j) = A * B(:,:,i,j);
    end
end


function bessel_y1_values_test ( )

%*****************************************************************************80
%
%% BESSEL_Y1_VALUES_TEST demonstrates the use of BESSEL_Y1_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSEL_Y1_VALUES_TEST:\n' );
  fprintf ( 1, '  BESSEL_Y1_VALUES stores values of\n' );
  fprintf ( 1, '  the Bessel Y1 function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X            Y1(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = bessel_y1_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function square_symq_rule_test01 ( degree, n )

%*****************************************************************************80
%
%% SQUARE_SYMQ_RULE_TEST01 calls SQUARESYMQ for a quadrature rule of given order.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    01 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer DEGREE, the desired total polynomial degree exactness
%    of the quadrature rule.
%
%    Input, integer N, the number of nodes.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SQUARE_SYMQ_RULE_TEST01\n' );
  fprintf ( 1, '  Symmetric quadrature rule for a square.\n' );
  fprintf ( 1, '  Polynomial exactness degree DEGREE = %d\n', degree );

  area = 4.0;
%
%  Retrieve and print a symmetric quadrature rule.
%
  [ x, w ] = square_symq ( degree, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes N = %d\n', n );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     J  W       X       Y\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %4d  %14.6g  %14.6g  %14.6g\n', j, w(j), x(1,j), x(2,j) );
  end

  d = sum ( w(1:n) );

  fprintf ( 1, '   Sum  %14.6g\n', d );
  fprintf ( 1, '  Area  %14.6g\n', area );

  return
end


function post_3d ( p, t )

%*****************************************************************************80
%
%% POST_3D is a postprocessor for the tetrahedronal mesh.
%
%  Copyright:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Modified:
%
%    31 July 2009
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, P(N,3), the coordinates of nodes.
%
%    Input, integer T(NT,4), the indices of nodes that make up the tetrahedrons.
%
  [ node_num, junk ] = size ( p );
  [ tetra_num, junk ] = size ( t );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  %d nodes were used.\n', node_num );
  fprintf ( 1, '  %d tetrahedrons were created.\n', tetra_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  QUALITY MEASURES:\n');
  fprintf ( 1, '  A value of 1.00 is best, and 0.00 is the worst.\n' );

  q = simp_qual_3d ( p, t, 1 );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Measure Min         Mean        Max         Var\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  #1  %10f  %10f  %10f  %10f\n', ...
    min ( q ), mean ( q ), max ( q ), var ( q ) );

  bad = find ( q == min ( q ) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The tetrahedron of minimum quality is number %d\n', bad(1) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Node        X             Y             Z\n' );
  fprintf ( 1, '\n' );

  for j = 1 : 4
    n = t(bad(1),j);
    fprintf ( 1, '  %4d  %12f  %12f  %12f\n',  n, p(n,1:3) );
  end

  return
end


function [ind,t0,s0,t0close,s0close] = crossing(S,t,level,imeth)
% CROSSING find the crossings of a given level of a signal
%   ind = CROSSING(S) returns an index vector ind, the signal
%   S crosses zero at ind or at between ind and ind+1
%   [ind,t0] = CROSSING(S,t) additionally returns a time
%   vector t0 of the zero crossings of the signal S. The crossing
%   times are linearly interpolated between the given times t
%   [ind,t0] = CROSSING(S,t,level) returns the crossings of the
%   given level instead of the zero crossings
%   ind = CROSSING(S,[],level) as above but without time interpolation
%   [ind,t0] = CROSSING(S,t,level,par) allows additional parameters
%   par = {'none'|'linear'}.
%	With interpolation turned off (par = 'none') this function always
%	returns the value left of the zero (the data point thats nearest
%   to the zero AND smaller than the zero crossing).
%
%	[ind,t0,s0] = ... also returns the data vector corresponding to
%	the t0 values.
%
%	[ind,t0,s0,t0close,s0close] additionally returns the data points
%	closest to a zero crossing in the arrays t0close and s0close.
%
%	This version has been revised incorporating the good and valuable
%	bugfixes given by users on Matlabcentral. Special thanks to
%	Howard Fishman, Christian Rothleitner, Jonathan Kellogg, and
%	Zach Lewis for their input.

% Steffen Brueckner, 2002-09-25
% Steffen Brueckner, 2007-08-27		revised version

% Copyright (c) Steffen Brueckner, 2002-2007
% brueckner@sbrs.net

% check the number of input arguments
error(nargchk(1,4,nargin));

% check the time vector input for consistency
if nargin < 2 || isempty(t)
	% if no time vector is given, use the index vector as time
    t = 1:length(S);
elseif length(t) ~= length(S)
	% if S and t are not of the same length, throw an error
    error('t and S must be of identical length!');
end

% check the level input
if nargin < 3
	% set standard value 0, if level is not given
    level = 0;
end

% check interpolation method input
if nargin < 4
    imeth = 'linear';
end

% make row vectors
t = t(:)';
S = S(:)';

% always search for zeros. So if we want the crossing of
% any other threshold value "level", we subtract it from
% the values and search for zeros.
S   = S - level;

% first look for exact zeros
ind0 = find( S == 0 );

% then look for zero crossings between data points
S1 = S(1:end-1) .* S(2:end);
ind1 = find( S1 < 0 );

% bring exact zeros and "in-between" zeros together
ind = sort([ind0 ind1]);

% and pick the associated time values
t0 = t(ind);
s0 = S(ind);

if strcmp(imeth,'linear')
    % linear interpolation of crossing
    for ii=1:length(t0)
        if abs(S(ind(ii))) > eps(S(ind(ii)))
            % interpolate only when data point is not already zero
            NUM = (t(ind(ii)+1) - t(ind(ii)));
            DEN = (S(ind(ii)+1) - S(ind(ii)));
            DELTA =  NUM / DEN;
            t0(ii) = t0(ii) - S(ind(ii)) * DELTA;
            % I'm a bad person, so I simply set the value to zero
            % instead of calculating the perfect number ;)
            s0(ii) = 0;
        end
    end
end

% Addition:
% Some people like to get the data points closest to the zero crossing,
% so we return these as well
[CC,II] = min(abs([S(ind-1) ; S(ind) ; S(ind+1)]),[],1);
ind2 = ind + (II-2); %update indices

t0close = t(ind2);
s0close = S(ind2);


function varargout = transformVector(varargin)
%TRANSFORMVECTOR Transform a vector with an affine transform.
%
%   VECT2 = transformVector(VECT1, TRANS);
%   where VECT1 has the form [xv yv], and TRANS is a [2*2], [2*3] or [3*3]
%   matrix, returns the vector transformed with affine transform TRANS.
%
%   Format of TRANS can be one of :
%   [a b]   ,   [a b c] , or [a b c]
%   [d e]       [d e f]      [d e f]
%                            [0 0 1]
%
%   VECT2 = transformVector(VECT1, TRANS);
%   Also works when PTA is a [N*2] array of double. In this case, VECT2 has
%   the same size as VECT1.
%
%   [vx2 vy2] = transformVector(vx1, vy1, TRANS);
%   Also works when vx1 and vy1 are arrays the same size. The function
%   transform each couple of (vx1, vy1), and return the result in
%   (vx2, vy2), which is the same size as (vx1 vy1).
%
%
%   See also
%   vectors2d, transforms2d, rotateVector, transformPoint

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2007-03-12
% Copyright 2007-2022 INRA - TPV URPOI - BIA IMASTE

if length(varargin)==2
    var = varargin{1};
    vx = var(:,1);
    vy = var(:,2);
    trans = varargin{2};
elseif length(varargin)==3
    vx = varargin{1};
    vy = varargin{2};
    trans = varargin{3};
else
    error('wrong number of arguments in "transformVector"');
end

% compute new position of vector
vx2 = vx*trans(1,1) + vy*trans(1,2);
vy2 = vx*trans(2,1) + vy*trans(2,2);

% format output
if nargout==0 || nargout==1
    varargout{1} = [vx2 vy2];
elseif nargout==2
    varargout{1} = vx2;
    varargout{2} = vy2;
end


%
% Perform the mean and variance normalization (MVN)
%       output = MVN(input,mean_d,var_d,weight)
%
% Input:
%   input: a feature vector or matrix to be processed. If the input is a
%          vector, it should be a column vector; if the input is a matrix,
%          it should be MxN, where M is the number of frames and N is the
%          feature index.
%   mean_d: the desired mean of the output. If the mean_d is not provided,
%           zero is used. If the input is a vector, mean_d should be a
%           scalar; if the input is a matrix, mean_d can be either a scalar
%           or a column vector.
%   var_d: the desired variance of the output. If the var_d is not provided,
%          one is used. If the input is a vector, var_d should be a
%          scalar; if the input is a matrix, var_d can be either a scalar
%          or a column vector.
%   weight: the weight of each frame during the normalization. The
%          function supports partial ownership, i.e. a frame may be owned by a
%          class partially. This is useful in multi-class MVN. If weight is not
%          provided, all frames will have weight 1, i.e. conventional MVN.
%
% Output:
%   output: the mean-variance-normalized features
%
% Author: Xiao Xiong, School of Computer Engineering, NTU, Singapore.
% Created: Jan, 2005
% Last Modified: 21 Dec, 2010
%
function [output] = MVN(input,mean_d,var_d, weight)
useWeight = 1;
if nargin < 4
    useWeight = 0;
end
if nargin < 3
    var_d = 1;  % default variance is 1
end
if nargin < 2
    mean_d = 0; % default mean is 0
end
[M, N] = size(input);   % Perform MVN for each column individually

if useWeight
    [meanX,varX] = findMeanVarainceWeighted(input, weight, 0);      % find the mean and variance of input weighted by their weights
    output = input - repmat(meanX',M,1);
    gain = diag(sqrt(var_d./varX));
    output = output * gain;
    output = output + repmat(mean_d',M,1);
else
    output = CVN(CMN(input,mean_d),var_d);
end


% Copyright (C) 1999 Paul Kienzle <pkienzle@users.sf.net>
% Copyright (C) 2006 Peter V. Lanspeary, <peter.lanspeary@.adelaide.edu.au>
%
% 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/>.

% -*- texinfo -*-
% @deftypefn  {Function File} {[@var{a}, @var{v}, @var{ref}] =} levinson (@var{acf})
% @deftypefnx {Function File} {[@dots{}] =} levinson (@var{acf}, @var{p})
%
% Use the Durbin-Levinson algorithm to solve:
%    toeplitz(acf(1:p)) * x = -acf(2:p+1).
% The solution [1, x'] is the denominator of an all pole filter
% approximation to the signal x which generated the autocorrelation
% function acf.
%
% acf is the autocorrelation function for lags 0 to p.
% p defaults to length(acf)-1.
% Returns
%   a=[1, x'] the denominator filter coefficients.
%   v= variance of the white noise = square of the numerator constant
%   ref = reflection coefficients = coefficients of the lattice
%         implementation of the filter
% Use freqz(sqrt(v),a) to plot the power spectrum.
%
% REFERENCE
% [1] Steven M. Kay and Stanley Lawrence Marple Jr.:
%   "Spectrum analysis -- a modern perspective",
%   Proceedings of the IEEE, Vol 69, pp 1380-1419, Nov., 1981
% @end deftypefn

% Based on:
%    yulewalker.m
%    Copyright (C) 1995 Friedrich Leisch <Friedrich.Leisch@ci.tuwien.ac.at>
%    GPL license

% FIXME: Matlab doesn't return reflection coefficients and
%        errors in addition to the polynomial a.
% FIXME: What is the difference between aryule, levinson,
%        ac2poly, ac2ar, lpc, etc.?

function [a, v, ref] = oc_levinson (acf, p)

if ( nargin<1 )
    print_usage;
elseif( ~isvector(acf) || length(acf)<2 )
    error( 'levinson: arg 1 (acf) must be vector of length >1\n');
elseif ( nargin>1 && ( ~isscalar(p) || fix(p)~=p ) )
    error( 'levinson: arg 2 (p) must be integer >0\n');
else
    if ((nargin == 1)||(p>=length(acf)))
        p = length(acf) - 1;
    end
    if( size(acf,2)>1 )
        acf=acf(:);
    end      % force a column vector

    if nargout < 3 && p < 100
        % direct solution [O(p^3), but no loops so slightly faster for small p]
        %   Kay & Marple Eqn (2.39)
        R = toeplitz(acf(1:p), conj(acf(1:p)));
        a = R \ -acf(2:p+1);
        a = [ 1, a.' ];
        v = real( a*conj(acf(1:p+1)) );
    else
        % durbin-levinson [O(p^2), so significantly faster for large p]
        %   Kay & Marple Eqns (2.42-2.46)
        ref = zeros(p,1);
        g = -acf(2)/acf(1);
        a = g;
        v = real( ( 1 - g*conj(g)) * acf(1) );
        ref(1) = g;
        for t = 2 : p
            g = -(acf(t+1) + a * acf(t:-1:2)) / v;
            a = [ a+g*conj(a(t-1:-1:1)), g ];
            v = v * ( 1 - real(g*conj(g)) ) ;
            ref(t) = g;
        end
        a = [1, a];
    end
end

end


%The spherical K-means algorithm
%(C) 2007-2011 Nguyen Xuan Vinh
%Contact: vinh.nguyenx at gmail.com   or vinh.nguyen at monash.edu
%Reference:
%   [1] Xuan Vinh Nguyen: Gene Clustering on the Unit Hypersphere with the
%       Spherical K-Means Algorithm: Coping with Extremely Large Number of Local Optima. BIOCOMP 2008: 226-233
%Usage: Normalize the data set to have mean zero and unit norm

function b=normalize_norm_mean(a)
[n dim]=size(a);
for i=1:n
    a(i,:)=(a(i,:)-mean(a(i,:)))/norm(a(i,:)-mean(a(i,:)),2);
end
b=a;