%% KUKA sunrise toolbox example.
% moving end-effector of the robot on an ellipse.

% Copy right: Mohammad SAFEEA
% 10th-Nov-2017
clear all;
close all;
clc;
% add the path of the KST to the working folder of Matlab
addpath getTheKSTDirectory(pwd)
% Initial configuration
jPos={0., pi / 180 * 30, 0, -pi / 180 * 60, 0,...
                        pi / 180 * 90, 0};
% Start the KST, move robot to initial configuration, start directServo function
[tKuka,flag]=connectToKuka(jPos );
% return;
if flag==false
    fprintf('Can not connect to KST \n');
    fprintf('Program terminated \n');
    return;
end

    % move a little bit back on the X direction
    deltaX=-60;deltaY=0;deltaZ=0.;
    Pos{1}=deltaX;
    Pos{2}=deltaY;
    Pos{3}=deltaZ;
    vel=50;
    movePTPLineEefRelBase( tKuka , Pos, vel);
    % put the pen on the level of the page
    deltaX=0;deltaY=0;deltaZ=-85.;
    Pos{1}=deltaX;
    Pos{2}=deltaY;
    Pos{3}=deltaZ;
    vel=50;
    movePTPLineEefRelBase( tKuka , Pos, vel);
    pause(1);
    % get joints angles of robot
    jPos  = getJointsPos( tKuka );

    % start the direct servo
     realTime_startDirectServoJoints(tKuka);

    % calculate current position of flange point of the robot
    qs=zeros(7,1);
for i=1:7
    qs(i)=jPos{i};
end
        TefTool=eye(4);
        T0=directKinematics(qs,TefTool); % EEF frame transformation matrix
        p0=T0(1:3,4);
        Tt=T0;

%% Define the ellipse, dimentsions are in (meter)
p=p0; % this is the starting point of the ellipse
c=p+([0; 50; 0]/1000); % this is the center of the ellipse
dir=[1 0 0]; % the direction vector of the (a) axis of the ellipse
ratio=0.5; % the radious ratio (a/b) of the ellipse
%% Claulcate ellipse parameters
[R,theta0,a,b,c,errorFlag]=getEllipseParameters(p,c,ratio,dir);
if errorFlag==true % this happens when the start point is on the (dir) vector direction.
    % in such case the ellipse plane can not be specified in space.
    % Stop the realtime motion
    realTime_stopDirectServoJoints(tKuka);
    % Turn off the server
    net_turnOffServer( tKuka );
    return;
end
%% Draw the length of the ellipse arc as a function of the parametric angle (theta)
theta=2*pi;

theta1=theta0+theta;

[ thetaVec,sVec ] = getEllipseLengthVector( a,b, theta0,theta1 );
sizesVec=max(size(sVec));
L=sVec(end); % L is the length of the curve

%% Calculate the times:
velocity=40/1000; % stable velcotiy, m/sec;
accel=25/1000; % linear acceleration, m/sec2
[t0,t1,t2]=calculateInterpolationTimes(L,velocity,accel);

%% Joint space control

    [Ttemp,J]=directKinematics(qs,TefTool);
    vec=Ttemp(1:3,4);

% dls solver parameters
        numberOfIterationForSolver=100;
        lambda=0.5;
        TefTool=eye(4);

    sCoordinate=0;
    interpolationCounter=1;
    thetaVar=theta0;

% start of time
        dateVector0=datevec(now);
        time0=dateVector0(6)+dateVector0(5)*60+dateVector0(4)*60*60; % calculate time at this instant
while true

    % Calculate the elapsed time
        dateVector=datevec(now);
        timeNow=dateVector(6)+dateVector(5)*60+dateVector(4)*60*60; % calculate time at this instant
        deltaT=timeNow-time0; % elapsed is zero at first excution

    % calculate position of servo point
    if deltaT<t0
        sCoordinate=0.5*accel*deltaT*deltaT;
    elseif(deltaT<t1)
        s0=0.5*accel*t0*t0;
        sCoordinate=velocity*(deltaT-t0)+s0;
    elseif(deltaT<t2)
           s0=0.5*accel*t0*t0;
           s1=velocity*(t1-t0)+s0;
           sCoordinate=velocity*(deltaT-t1)-0.5*accel*(deltaT-t1)*(deltaT-t1)+s1;
    end
 %% when time ends break the loop
    if deltaT>t2
        break;
    end
%% Interpolate theta from the sVec,thetaVec
    for counter=interpolationCounter:(sizesVec-1)
        if(sCoordinate>sVec(counter))
            tetaRange=thetaVec(counter+1)-thetaVec(counter);
            sRange=sVec(counter+1)-sVec(counter);
            thetaVar=(sCoordinate-sVec(counter))...
                *tetaRange/sRange+thetaVec(counter);
            interpolationCounter=counter;
        end
    end

    pPrime=R*ellipseParametricFunction( a,b,thetaVar );
    p=c+pPrime;

    % calculate target transform
    Tt(1:3,4)=p;

    [ qs ] = kukaDLSSolver( qs, Tt, TefTool,numberOfIterationForSolver,lambda );

    for i=1:7
        jPos{i}=qs(i);
    end

	%% Send joint positions to robot
	sendJointsPositions( tKuka,jPos);

end

    % Stop the realtime motion
    realTime_stopDirectServoJoints(tKuka);

    % put the pen up the page
    deltaX=0;deltaY=0;deltaZ=+85.;
    Pos{1}=deltaX;
    Pos{2}=deltaY;
    Pos{3}=deltaZ;
    vel=50;
    movePTPLineEefRelBase( tKuka , Pos, vel);

% Turn off the server
    net_turnOffServer( tKuka );
    fclose(tKuka);


function imageplot(M,str, a,b,c)

% imageplot - diplay an image and a title
%
% Example of usages:
%   imageplot(M);
%   imageplot(M,title);
%   imageplot(M,title,1,2,1);   % to make subplot(1,2,1);
%
%   If you want to display several images:
%       imageplot({M1 M2}, {'title1', 'title2'});
%
%   Copyright (c) 2007 Gabriel Peyre

if nargin<2
    str = [];
end

nbdims = 2;
if size(M,1)==1 || size(M,2)==1
    nbdims = 1;
end

if iscell(M)
    q = length(M);
    if nargin<5
        c = 1;
        a = ceil(q/4);
        b = ceil(q/a);
    end
    if (c-1+q)>(a*b)
        warning('a and c parameters not large enough');
        a = ceil((c-1+q)/4);
        b = ceil((c-1+q)/a);
    end
    for i=1:q
        if iscell(str)
            str1 = str{i};
        else
            str1 = str;
        end
        imageplot(M{i},str1, a,b,c-1+i);
    end
    global axlist;
    if not(isempty(axlist))
        linkaxes(axlist, 'xy');
    end
    return;
end

if nargin==5
    global axlist;
    global imageplot_size;
    if c==1 || isempty(imageplot_size) || imageplot_size~=size(M,1)
        clear axlist;
        global axlist;
        axlist = [];
        imageplot_size = size(M,1);
    end
    axlist(end+1) = subplot(a,b,c);
end

if nbdims==1
    plot(M); axis tight;
else
    if size(M,3)==2
        M = cat(3,M, zeros(size(M,1),size(M,2)));
    end
    if size(M,3)==1
        colormap gray(256);
    else
        colormap jet(256);
    end
    imagesc(rescale(M)); axis image; axis off;
end
if not(isempty(str))
    title(str);
end

if nargin==5 && c==a*b
    linkaxes(axlist, 'xy');
end


clc;

%% compile
% mex kdtree_build.cpp
% mex kdtree_k_nearest_neighbors.cpp
% disp('compiled.');

%% create data and execute query
rand('seed',1);
p = rand( 30, 2 ); % input data
q = [.5,.5]; % query data
tree = kdtree_build( p );
idxs = kdtree_k_nearest_neighbors(tree,q,10);

%% visualize
close all;
xlim( [0 1] );
ylim( [0 1] );
hold on; axis equal; axis off;
plot( p(:,1), p(:,2), '.b');
plot(q(1), q(2),'.r');
plot(p(idxs,1), p(idxs,2),'or');
legend('database', 'query', 'query results');

%% text lables
for i=1:numel(idxs)
    text(p(idxs(i),1), p(idxs(i),2),sprintf(' %d',i));
end


%TIME_FAC_VS_FB
%
%   The purpose of this test is to compare the fastest of the factorization
%   routines against the fastest of the filter bank routines for different
%   problems.
%
%   Some results: The FB computation seems to be completely memory bound:
%   increasing the length of the window does not impact the running time.
%
%   For the common 2xoversampling case with a long signal, the
%   factorization routine falls thorugh and is more than 10 times slower,
%   for the other case it is 3 times slower.

Lr=[480000*sf^2,480000*sf^2,262144*sf^2,262144*sf^2,900*sf^2,600];
ar=[     600*sf,     600*sf,        512,        512,       2, 20];
Mr=[     800*sf,     800*sf,       1024,       1024,  600*sf, 30];
gr=[     800*sf,480000*sf^2,       1024,     40*512,  600*sf,400];
Wr=[          2,          2,          2,          2,       1,600];

for ii=1:length(Lr)

  L=Lr(ii);

  M=Mr(ii);
  a=ar(ii);

  gl=gr(ii);
  W=Wr(ii);

  N=L/a;
  c=gcd(a,M);
  p=a/c;
  q=M/c;
  d=N/q;

  disp(sprintf('L:%6i W:%2i a:%4i M:%4i gl:%4i c:%4i d:%4i',L,W,a,M,gl,c,d));

  f=rand(L,W);
  gfir=rand(gl,1);
  c2=mex_dgt_fb_2(f,gl,a,M,1);

  f=rand(L,W);
  gf=rand(p*q,c*d);
  c1=mex_dgt_fac_7(f,gf,a,M,1);

end;


function newnode=sms(node,face,iter,alpha,method)
%
% newnode=sms(node,face,iter,useralpha,method)
%
% simplified version of surface mesh smoothing
%
% author: Qianqian Fang (fangq<at> nmr.mgh.harvard.edu)
% date: 2009/10/21
%
% input:
%    node:  node coordinates of a surface mesh
%    face:  face element list of the surface mesh
%    iter:  smoothing iteration number
%    alpha: scaler, smoothing parameter, v(k+1)=alpha*v(k)+(1-alpha)*mean(neighbors)
%    method: same as in smoothsurf, default is 'laplacianhc'
%
% output:
%    newnode: output, the smoothed node coordinates
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

if(nargin<5)
   method='laplacianhc';
end
if(nargin<4)
   if(nargin<3)
      iter=10;
   end
   alpha=0.5;
end

conn=meshconn(face,size(node,1));
newnode=smoothsurf(node(:,1:3),[],conn,iter,alpha,method,alpha);


function h=m_quiver(long,lat,u,v,varargin)
% M_QUIVER Makes a quiverplot on a map (QUIVER-style)
%    M_QUIVER(LONG,LAT,U,V) plots velocity vectors as arrows with components
%    (U,V) at the points (LONG,LAT) on the currently defined map.  The
%    matrices LONG,LAT,U,V must all be the same size. U and V contain the
%    eastward and northward components of velocity. Arrow scaling is automatic.
%
%    M_QUIVER(X,Y,U,V,S) automatically scales the arrows to fit within the
%    grid and then stretches them by S.  Use S=0 to plot the arrows without
%    the automatic scaling; In this case the scaling is 1 unit/degree
%    latitude. Note that we do not scale arrows with respect to map
%    coordinates! Instead, the arrows will correspond better to actual motions
%    over some time step. The tradeoff is that a single scale arrow cannot
%    be accurate for the entire map (M_VEC scales arrows according to
%    map coordinates).
%
%    M_QUIVER(...,LINESPEC) uses the plot linestyle specified for
%    the velocity vectors.  Any marker in LINESPEC is drawn at the base
%    instead of an arrow on the tip.  Use a marker of '.' to specify
%    no marker at all.  See PLOT for other possibilities. M_QUIVER is a wrapper
%    for QUIVER - for fancier arrows it is possible to replace the call to
%    QUIVER with one to another routine that draws fancy arrows, e.g.
%    ARROW (from TMW user-contrib software archive), or to use M_VEC.
%
%    M_QUIVER(...,'filled') fills any markers specified.
%
%    H = M_QUIVER(...) returns a vector of line handles.
%
%    See also QUIVER, M_VEC

% Rich Pawlowicz (rich@ocgy.ubc.ca) 20/Jan/97
%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.
%

% 6/Nov/00 - eliminate returned stuff if ';' neglected (thx to D Byrne)

global MAP_PROJECTION MAP_VAR_LIST

% Have to have initialized a map first

if isempty(MAP_PROJECTION)
  disp('No Map Projection initialized - call M_PROJ first!');
  return;
end

[X,Y]=m_ll2xy(long,lat,'clip','point');

[XN,YN]=m_ll2xy(long,lat+.01,'clip','point');
[XE,YE]=m_ll2xy(long+(.01)./cos(lat*pi/180),lat,'clip','point');

mU=u.*(XE-X)*100 + v.*(XN-X)*100;
mV=u.*(YE-Y)*100 + v.*(YN-Y)*100;

h=quiver(X,Y,mU,mV,varargin{:});
set(h,'tag','m_quiver');

if nargout==0
 clear h
end


function data = simulate_flight(setup)

%This function simulates the flight phase of the dynamics

%Set up the integration algorithm
Tspan = setup.Tspan;
IC = [  setup.IC.th;
    setup.IC.x;
    setup.IC.y;
    setup.IC.dth;
    setup.IC.dx;
    setup.IC.dy];
eventFunc = @(t,z)events_flight(t,z,setup.P);
options = odeset(...
    'RelTol',setup.tol,...
    'AbsTol',setup.tol,...
    'Vectorized','on',...
    'MaxStep',setup.odeMaxStep,...
    'Events',eventFunc);
userfun = @(t,z)dynamics_flight(t,z,setup.P);

%Run simulation
sol = feval(setup.solver,userfun,Tspan,IC,options);

%Format for post processing
tspan = [sol.x(1),sol.x(end)];
nTime = ceil(setup.dataFreq*diff(tspan));
t = linspace(tspan(1),tspan(2),nTime);
Z = deval(sol,t);
[~, E] = dynamics_flight(t,Z,setup.P);

%Store in a nice format for plotting
data.time = t;
data.state.th = Z(1,:);
data.state.x = Z(2,:);
data.state.y = Z(3,:);
data.state.dth = Z(4,:);
data.state.dx = Z(5,:);
data.state.dy = Z(6,:);
data.contact.h = zeros(1,nTime);
data.contact.v = zeros(1,nTime);
data.energy.potential = E(1,:);
data.energy.kinetic = E(2,:);
data.P = setup.P;

%Get transitions for finite state machine
data.phase = 'FLIGHT';
if isempty(sol.ie)
    data.exit = 'TIMEOUT';
else
    switch sol.ie(end)
        case 1   %close end of the stick hit the ground
            data.exit = 'STRIKE_0';
        case 2   %Far end of the stick hit the ground
            data.exit = 'STRIKE_2';
        otherwise
            error('Invalid exit condition in simulate flight')
    end
end

end


function [ parent_pop ] = evaluate_pop( parent_pop, obj_func )
%   This function evaluates a whole population with the supplied
%   objective function handle obj_func.

global nreal ;
global nobj ;
global ncon ;

obj_col = nreal + 1: nreal + nobj ;
parent_pop(:,obj_col) = 0 ;
parent_pop = obj_func(parent_pop);

if(ncon > 0)
    cv_col = nreal + nobj + ncon + 1 ;
    parent_pop(:,cv_col) = 0;
    cons = parent_pop(:,nreal+nobj+1:nreal+nobj+ncon);
    % parent_pop(:,cv_col) = sum(((cons < 0.0) .* cons),2);
    parent_pop(:,cv_col) = sum(bsxfun(@times, ...
                                    bsxfun(@lt, cons, 0.0), cons),2);
end
end


function cordic_test ( )

%*****************************************************************************80
%
%% CORDIC_TEST tests the CORDIC library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 June 2007
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CORDIC_TEST\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Test the CORDIC library.\n' );

  cordic_test001 ( );
  cordic_test002 ( );
  cordic_test003 ( );
  cordic_test004 ( );
  cordic_test005 ( );
  cordic_test006 ( );
  cordic_test007 ( );
  cordic_test008 ( );
  cordic_test009 ( );
  cordic_test010 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CORDIC_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function [gout,a,fc,L,info] = waveletfilters(Ls, scales, varargin)
%WAVELETFILTERS Generates wavelet filters
%   Usage: [gout,a,fc,L,info] = waveletfilters(Ls,scales)
%          [gout,a,fc,L,info] = waveletfilters(Ls,'bins', fs, fmin, fmax, bins)
%          [gout,a,fc,L,info] = waveletfilters(Ls,'linear', fs, fmin, fmax, channels)
%
%   Input parameters:
%         Ls     : System length
%         scales : Vector of wavelet scales
%   Output parameters:
%         gout  : Cell arrary of wavelet filters
%         a     : Downsampling rate for each channel.
%         fc    : Center frequency of each channel.
%         L     : Next admissible length suitable for the generated filters.
%         info  : Struct with additional outputs
%
%   `waveletfilters(Ls,scales)` constructs a system of wavelet filters covering
%   scales in the range *scales* for system length *Ls*. A scale of 1 corresponds
%   to a wavelet filter with peak positioned at the frequency 0.1 relative
%   to the Nyquist rate.
%
%   `[g,a,fc]=waveletfilters(Ls, 'bins', fs,fmin,fmax,bins)` constructs a set of
%   wavelets *g* which cover the required frequency range
%   `fmin`-`fmax` with `bins` filters per octave starting at `fmin`. All
%   filters have (approximately) equal $Q=f_c/f_b$. The
%   frequency interval below fmin not covered by these is captured
%   by additional lowpass filter(s) . The signal length *Ls*
%   is mandatory, since we need to avoid too narrow frequency windows
%
%   `[g,a,fc]=waveletfilters(Ls, 'linear', fs, fmin, fmax, channels)` constructs
%    a set of wavelets *g* which cover the required frequency range
%   `fmin`-`fmax` with `channels` equidistantly spaced filters starting at `fmin`.
%
%   Wavelet types
%   --------------------
%
%   The following wavelets can be passed as a flag:
%
%   'cauchy'     Cauchy wavelet (default parameters [alpha beta gamma] = [300 0 3])
%
%   'morse'      Generalized morse wavelet (default parameters [alpha beta gamma] = [300 0 3])
%
%   'morlet'     Morlet wavelet (default parameters sigma = [4])
%
%   'fbsp'       Frequency B-spline wavelet (default parameters [order fb] = [4 2])
%
%   'analyticsp' Analytic spline wavelet (default parameters [order fb] = [4 2])
%
%   'cplxsp'     Complex spline wavelet (default parameters [order fb] = [4 2])
%
%   A scale of 1 corresponds
%   to a wavelet filter with peak positioned at the frequency 0.1 relative
%   to the Nyquist rate. By default, this function does not allow center frequencies
%   exceeding the Nyquist rate, except with the optional parameter 'analytic',
%   see below. This implies that scales below 0.1 (or 0.05 for the 'analytic' scheme)
%   are not supported.
%   For more details on the construction of the wavelets and the available
%   wavelet types, please see |freqwavelet|.
%
%   By default, wavelet filters are peak normalized before being adjusted
%   to the proposed downsampling factor. The peak normalization can be
%   overridden by forwarding any norm flag accepted by |setnorm|.
%
%   Downsampling factors
%   --------------------
%
%   The integer downsampling rates of the channels must all divide the
%   signal length, |filterbank| will only work for input signal lengths
%   being multiples of the least common multiple of the downsampling rates.
%   See the help of |filterbanklength|.
%   The fractional downsampling rates restrict the filterbank to a single
%   length *L=Ls*.
%
%   `[gout,a]=waveletfilters(...,'regsampling')` constructs a non-uniform
%   filterbank with integer subsampling factors. This is the default.
%
%   `[gout,a]=waveletfilters(...,'uniform')` constructs a uniform filterbank
%   where the integer downsampling rate is the same for all the channels. This
%   results in the most redundant representation which produces nice plots.
%
%   `[gout,a]=waveletfilters(...,'fractional')` constructs a filterbank with
%   fractional downsampling rates *a*.
%   This results in the least redundant system.
%
%   `[gout,a]=waveletfilters(...,'fractionaluniform')` constructs a filterbank
%   with fractional downsampling rates *a*, which are uniform for all filters
%   except the "filling" low-pass filter which can have different
%   fractional downsampling rates. This is useful when uniform subsampling
%   and low redundancy at the same time are desirable.
%
%   Lowpass filters
%   --------------------
%
%   `[gout,a]=waveletfilters(...,'single')` uses a single lowpass filter
%   for covering the range from zero frequency to the center frequency of the
%   largest scale specified. This is the default.
%
%   `[gout,a]=waveletfilters(...,'repeat')` constructs frequency-shifted
%   copies of the largest scale wavelet to cover the range from zero frequency
%   to the center frequency of the largest scale specified.
%
%   `[gout,a]=waveletfilters(...,'none')` foregoes the construction of a
%   lowpass filter. This option cannot be expected to yield an invertible
%   filterbank.
%
%   Additional parameters
%   ---------------------
%
%   `waveletfilter` accepts the following optional parameters:
%
%     'redmul',redmul    Redundancy multiplier. Increasing the value of this
%                        will make the system more redundant by lowering the
%                        channel downsampling rates. It is only used if the
%                        filterbank is a non-uniform filterbank. Default
%                        value is *1*. If the value is less than one, the
%                        system may no longer be painless.
%
%     'redtar',redtar    Target redundancy. The downsampling factors will be
%                        adjusted to achieve a redundancy as close as possible
%                        to 'redtar'.
%
%     'trunc_at',trunc_at     Applies hard thresholding of the wavelet filters
%                             at the specified threshold value to reduce their
%                             support size.
%                             The default value is *trunc_at=10e-5*. When no
%                             truncation is desired, *trunc_at=0* should be chosen.
%
%     'delay',delay      A scalar, numeric vector of function handle that
%                        specifies delays for the wavelet filters. A
%                        numeric vector must have at least as many entries
%                        as there are filters in the filterbank. A function
%                        handle must accept two inputs *(k-1,a(k))*, where
%                        *k* is the channel index and *a* are the
%                        downsampling rates. If a function handle is given
%                        and 'redtar' is specified, delays are computed
%                        based on the final value of *a*.
%
%     'real'             Allows positive scales with center frequencies up
%                        to Nyquist. This is the default.
%
%     'complex'          Allows positive scales with center frequencies up
%                        to Nyquist, which are also mirrored to cover
%                        negative scales.
%
%     'analytic'         Allows positive scales with center frequencies up
%                        to twice the Nyquist frequency. This setting is
%                        suitable for the analysis of analytic signals.
%
%     'startfreq'        Allows to manually set a starting frequency for
%                        the wavelet range. Can not be lower than fmin.
%
%   Examples:
%   ---------
%
%   In the first example, we analyze a glockenspiel signal with a
%   regularly sampled wavelet filterbank using a frequency B-spline
%   wavelet of order 4 and with parameter fb=3 and visualize the result:::
%
%     [f,fs]=gspi;  % Get the test signal
%     Ls = length(f);
%     scales = linspace(10,0.1,100);
%     [g,a,fc,L]=waveletfilters(Ls,scales, {'fbsp', 4, 3}, 'repeat');
%     c=filterbank(f,g,a);
%     plotfilterbank(c,a,fc,fs,90);
%
%   In the second example, we construct a wavelet filterbank with a
%   lowpass channels based on a Cauchy wavelet and verify it.
%   The plot shows the frequency responses of
%   filters used for analysis (top) and synthesis (bottom). :::
%
%     [f,fs]=greasy;  % Get the test signal
%     Ls = length(f);
%     M0 = 511; %Desired number of channels (without 0 Hz-lowpass channel)
%     max_freqDiv10 = 10;  % 10 corresponds to the nyquist frequency
%     freq_step = max_freqDiv10/M0;
%     rate = 44100;
%     start_index = 1;
%     min_freqHz = rate/10*freq_step
%     min_scale_freq = min_freqHz*start_index
%     min_freqDiv10 = freq_step*start_index; %1/25; % By default, the reference scale for freqwavelet has center frequency 0.1
%     scales = 1./linspace(min_freqDiv10,max_freqDiv10,M0);
%     alpha = 1-2/(1+sqrt(5)); % 1-1/(goldenratio) delay sequence
%     delays = @(n,a) a*(mod(n*alpha+.5,1)-.5);
%     CauchyAlpha = 600;
%     [g, a,fc,L,info] = waveletfilters(Ls,scales,{'cauchy',CauchyAlpha},'uniform','single','energy', 'delay',delays, 'redtar', 8);
%
%     c=filterbank(f,{'realdual',g},a);
%     r=2*real(ifilterbank(c,g,a));
%     if length(r) > length(f)
%         norm(r(1:length(f))-f)
%     else
%         norm(r-f(1:length(r)))
%      end
%     % Plot frequency responses of individual filters
%     gd=filterbankrealdual(g,a,L);
%     figure(1);
%     subplot(2,1,1);
%     filterbankfreqz(gd,a,L,fs,'plot','linabs','posfreq');
%
%     subplot(2,1,2);
%     filterbankfreqz(g,a,L,fs,'plot','linabs','posfreq');
%
%   See also: freqwavelet, filterbank, setnorm

% AUTHORS: Nicki Holighaus, Zdenek Prusa, Guenther Koliander, Clara Hollomey

complainif_notenoughargs(nargin,2,upper(mfilename));
complainif_notposint(Ls,'Ls',upper(mfilename));

%parse input arguments:
if ~isnumeric(scales)
    fs = varargin{1};
    fmin = varargin{2};
    fmax = varargin{3};
    channels = varargin{4};
    switch scales
        case 'linear'
            definput.flags.inputmode = {'linear', 'logarithmic', 'bins', 'scales'};
        %case 'logarithmic'
        %    definput.flags.inputmode = {'logarithmic', 'bins', 'scales', 'linear'};
        case 'bins'
            definput.flags.inputmode = {'bins', 'scales', 'linear', 'logarithmic'};
        otherwise
            error('%s: second argument must either be a scales vector or define the f-mapping.',upper(mfilename))
    end
    %this is a slightly more efficient way to remove the first 4 args from
    %varargin
    varargin = circshift(varargin,-4);
    varargin = varargin(1:end-4);
else
    definput.flags.inputmode = {'scales', 'linear', 'logarithmic', 'bins'};
end

definput.import={'setnorm'};
definput.importdefaults={'null'};
definput.flags.real = {'real','complex','analytic'};
definput.flags.lowpass  = {'single','repeat','none'};
definput.flags.sampling = {'regsampling','uniform',...
                           'fractional','fractionaluniform'};
definput.flags.wavelettype = getfield(arg_freqwavelet(),'flags','wavelettype');
definput.keyvals.redmul=1;
definput.keyvals.redtar=[];
definput.keyvals.delay = 0;
definput.keyvals.trunc_at  = 10^(-5);
definput.keyvals.fs = 2;
definput.keyvals.startfreq = [];%only relevant if 'repeat'

[varargin,winCell] = arghelper_filterswinparser(definput.flags.wavelettype,varargin);
[flags,kv]=ltfatarghelper({},definput,varargin);

if isempty(winCell), winCell = {flags.wavelettype}; end

if ~isa(kv.delay,'function_handle') && ~isnumeric(kv.delay)
    error('%s: delay must be a function handle or numeric.',upper(mfilename));
end

if ~isscalar(kv.redmul) || kv.redmul <= 0
    error('%s: redmul must be a positive scalar.',upper(mfilename));
end

if ~isempty(kv.redtar)
    if ~isscalar(kv.redtar) || kv.redtar <= 0
        error('%s: redtar must be a positive scalar.',upper(mfilename));
    end
end

%parse the input format: map fmin and fmax to scales according to the input
%parameter specification
if ~flags.do_scales
    nf = fs/2;
    if flags.do_linear
        min_freq = fmin/nf *10;%map to freqwavelets nyquist f
        max_freq = fmax/nf * 10;
        scales = 1./linspace(min_freq,max_freq,channels);
  % elseif flags.do_logarithmic
%
%        fc = 2.^linspace(log2(fmin), log2(fmax), channels);
%        fc = fc/nf * 10;
%        scales = 1./fc;

    elseif flags.do_bins

        if isscalar(channels)
            % Number of octaves
            b = ceil(log2(fmax/fmin))+1;
            bins = channels*ones(b,1);
        else
            bins = channels;
        end

        fc = zeros(sum(bins),1);

        ll = 0;
        for kk = 1:length(bins)
            fc(ll+(1:bins(kk))) = ...
                fmin*2.^(((kk-1)*bins(kk):(kk*bins(kk)-1)).'/bins(kk));
            ll = ll+bins(kk);
        end

        % Get rid of filters with frequency centers >=fmax and nf
        % This will leave the first bigger than fmax it it is lower than nf
        temp = find(fc>=fmax ,1);
        if fc(temp) >= nf
            fc = fc(1:temp-1);
        else
            fc = fc(1:temp);
        end

        channels = length(fc);
        min_freq = fmin/nf *10;%map to freqwavelets nyquist f
        max_freq = fmax/nf * 10;
        scales = 1./linspace(min_freq,max_freq,channels);
    end
    scales_sorted = sort(scales,'descend');
    if ~isempty(kv.startfreq)%set the start frequency
        startfreq = kv.startfreq/nf * 10;
        scales_start = find(1./scales_sorted > startfreq,1,'first');%find first scale whose equiv. f is larger than fmin
        scales = scales(scales_start:end);
    end
end

if ~isnumeric(scales) || any(scales < 0.1)
   error('%s: scales must be positive and numeric.',upper(mfilename));
end

if size(scales,2)>1
    if size(scales,1)==1
        % scales was a row vector.
        scales=scales(:);
    else
        error('%s: scales must be a vector.',upper(mfilename));
    end
end

%% Generate mother wavelet to determine parameters from
[~,info] = freqwavelet(winCell,Ls,1,'asfreqfilter','efsuppthr',kv.trunc_at,'basefc',0.1);
basea = info.aprecise;

%% Determine total number of filters and natural subsampling factor for lowpass
%[aprecise, M, lowpass_number, lowpass_at_zero] = c_det_lowpass(Ls, scales, basea, flags, kv);
if numel(scales) < 4 && flags.do_single
    error('%s: Lowpass generation requires at least 4 scales.',upper(mfilename));
elseif numel(scales) < 2 && flags.do_repeat
    error('%s: Lowpass generation requires at least two scales.',upper(mfilename));
end

% Get number of scales and sort them
M = numel(scales);
scales_sorted = sort(scales,'descend');
%% Determine total number of filters and natural subsampling factor for lowpass
if flags.do_repeat
% Maybe adjust this to not guarantee some distance between first filter and zero frequency.
    lowpass_number = scales_sorted(2)/(scales_sorted(1)-scales_sorted(2));
        if abs(lowpass_number - round(lowpass_number)) < eps*10^3
            % determine if lowpass is centered around 0 Hz
            lowpass_number = round(lowpass_number);
            lowpass_at_zero = 1;
        else
            lowpass_at_zero = 0;
        end
        lowpass_number = floor(lowpass_number);
        if lowpass_number == 0
            lowpass_number = 1;
        end
        M = M + lowpass_number;
        aprecise = (basea.*scales_sorted(1))*ones(lowpass_number,1);

elseif flags.do_single
    lowpass_number = 1;
    lowpass_at_zero = 1;
    M = M+1;
    %this is an estimated value
    aprecise = (0.2./scales_sorted(4))*Ls; % This depends on how single lowpass is called (l.195ff). Maybe automate. Adapt if necessary!!!
else
    lowpass_number = 0;
    lowpass_at_zero = 0;
    aprecise = [];
end

%% Get subsampling factors
aprecise = [aprecise;basea.*scales];

if any(aprecise<1)
    error(['%s: Bandwidth of at least one of the filters is bigger than fs. '],upper(mfilename));
end

aprecise=aprecise/kv.redmul;
if any(aprecise<1)
    error('%s: The maximum redundancy mult. for this setting is %5.2f',...
         upper(mfilename), min(basea./scales));
end

%% Compute the downsampling rate
if flags.do_regsampling
    a = ones(M,1);

    [lower_scale,~] = max(scales);
    [upper_scale,~] = min(scales);
    lower_scale = floor(log2(1/lower_scale));
    upper_scale = floor(log2(1/upper_scale));

    % Find minimum a in each octave and floor23 it
    % to shrink "a" to the next composite number
    ct=1;
    for kk = lower_scale:upper_scale
        tempidx = find( floor(log2(1./scales)) == kk );
        [~,tempminidx] = min(1/scales(tempidx));
        idx = tempidx(tempminidx);

        % Deal the integer subsampling factors
        a(tempidx) = floor23(aprecise(idx));
        ct=ct+1;
    end

    % Determine the minimal transform length lcm(a)
    L = filterbanklength(Ls,a);

    % Heuristic trying to reduce lcm(a)
    while L>2*Ls && ~(all(a==a(1)))
        maxa = max(a);
        a(a==maxa) = 0;
        a(a==0) = max(a);
        L = filterbanklength(Ls,a);
    end

elseif flags.do_fractional
    L = Ls;
    N=ceil(Ls./aprecise);
    a=[repmat(Ls,M,1),N];
elseif flags.do_fractionaluniform
    L = Ls;
    if lowpass_at_zero
        aprecise(2:end)= min(aprecise(2:end));
    else
        aprecise= repmat(min(aprecise),numel(aprecise),1);
    end
    N=ceil(Ls./aprecise);
    a=[repmat(Ls,M,1),N];
elseif flags.do_uniform
    a=floor(min(aprecise));
    L=filterbanklength(Ls,a);
    a = repmat(a,M,1);
end
% Get an expanded "a" / Convert "a" to LTFAT 2-column fractional format
afull=comp_filterbank_a(a,M,struct());

%if flags.do_uniform
%    a = a(:,1);
%end
%==========================================================================
%% Adjust the downsampling rates in order to achieve 'redtar'

if ~isempty(kv.redtar)
   if size(afull,2) == 2
        a = afull(:,1)./afull(:,2);
   else
       a = afull;
   end

    if ~flags.do_real
        org_red = sum(1./a);
    elseif lowpass_at_zero
        org_red = 1./a(1) + sum(2./a(2:end));
    else
        org_red = sum(2./a);
    end

    a = floor(a*org_red/kv.redtar);
    a(a==0) = 1;

    if ~flags.do_uniform
        N_new=ceil(L./a);
        if flags.do_complex
            N_new = [N_new;N_new(end:-1:2)];
        end
        a=[repmat(L,numel(N_new),1),N_new];
    else
        L = filterbanklength(L,a);
        a=[a,ones(length(a), 1)];
    end
else
    a = afull;
end

%% Compute the scaling of the filters and the numeric delay vector
% Filters are scaled such that the energy of the subband coefficients
% remains approximately constant independent of the decimation factor
if isa(kv.delay,'function_handle')
    delayvec = zeros(M,1);
    for kk = 1:M
        delayvec(kk) = kv.delay(kk-1,a(kk,1)./a(kk,2));
    end
elseif numel(kv.delay) == 1
    delayvec = repmat(kv.delay,M,1);
elseif ~isempty(kv.delay) && size(kv.delay,2) > 1
    delayvec = kv.delay(:);
else
    error('%s: delay must be scaler or have enough elements to cover all channels.',upper(mfilename));
end
scal=sqrt(a(:,1)./a(:,2));

if flags.do_complex

    if lowpass_at_zero
       a=[a;flipud(a(2:end,:))];
       scal=[scal;flipud(scal(2:end))];
       delayvec=[delayvec;flipud(delayvec(2:end))];
    else
        a=[a;flipud(a)];
        scal=[scal;flipud(scal)];
        delayvec=[delayvec;flipud(delayvec)];
    end

    [gout_positive,info_positive] = freqwavelet(winCell,L,scales,...
        'asfreqfilter','efsuppthr',kv.trunc_at,'basefc',0.1,...
        'scal',scal(lowpass_number+1:M),'delay', delayvec(lowpass_number+1:M),flags.norm);
    [gout_negative,info_negative] = freqwavelet(winCell,L,-flipud(scales),...
        'asfreqfilter','efsuppthr',kv.trunc_at,'basefc',0.1,...
        'negative','scal',scal(M+1:M+numel(scales)),'delay', delayvec(M+1:M+numel(scales)), flags.norm);
    gout = [gout_positive,gout_negative];
    fields = fieldnames(info_positive);
    info = struct();
    for kk = 1:length(fields)
            info.(fields{kk}) = [info_positive.(fields{kk}),info_negative.(fields{kk})];
    end
elseif flags.do_analytic
    [gout,info] = freqwavelet(winCell,L,scales,...
        'asfreqfilter','efsuppthr',kv.trunc_at,'basefc',0.1,...
        'analytic','scal',scal(lowpass_number+1:M),'delay', delayvec(lowpass_number+1:M),flags.norm);
else
    if lowpass_at_zero
        % Scale the lowpass filters
        scal(1)=scal(1)/sqrt(2);
    end

    [gout,info] = freqwavelet(winCell,L,scales,'asfreqfilter','efsuppthr',...
        kv.trunc_at,'basefc',0.1,'scal',scal(lowpass_number+1:M),'delay', delayvec(lowpass_number+1:M),flags.norm);
end

%% Generate lowpass filters if desired
[gout, info] = comp_fblowpassfilters(winCell, gout, a, L, info, scales, scal, delayvec(1:lowpass_number), lowpass_at_zero, kv, flags);

info.lowpassstart = lowpass_number + 1;%startindex of actual wavelets (tentative)
% Assign fc and adjust for sampling rate
if flags.do_scales
    fc = (kv.fs/2).*info.fc;
else
    fc = nf.*info.fc;
end

if flags.do_uniform || flags.do_regsampling
    a = a(:,1);
end


function [x, infos] = sagmu_nmf(V, rank, in_options)
% Stochastic averaging gradient multiplicative update for non-negative matrix factorization (SAGMU-NMF) algorithm.
%
% Inputs:
%       matrix      V
%       rank        rank
%       options     options
% Output:
%       w           solution of w
%       infos       information
%
%
% Reference:
%       H. Kasai,
%       "Accelerated stochastic multiplicative update with gradient averaging for nonnegative matrix factorizations,"
%       EUSIPCO, 2018.
%
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai on March 22, 2017
%
%       Feb. 26, 2018 (Hiroyuki Kasai): Fixed algorithm.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

    m = size(V, 1);
    n = size(V, 2);

    % set local options
    local_options.fast_calc             = true;
    local_options.permute_on            = true;
    local_options.sub_mode              = 'STD';
    local_options.accel                 = false;
    local_options.ls                    = false;
    local_options.h_repeat              = 1;
    local_options.rep_mode              = 'fix';
    local_options.stepsize_ratio        = 1; % stepsize ratio
    local_options.robust                = false;
    local_options.lambda                = 1;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    number_of_batches = floor(n/options.batch_size);

    if ~isfield(options, 'accel')
        options.accel = false;
        if options.ls
            options.sub_mode = 'LS';
        else
            options.sub_mode = 'STD';
        end
    else
        if options.accel
            options.sub_mode = 'ACC';
        else
            if options.ls
                options.sub_mode = 'LS';
            else
                options.sub_mode = 'STD';
            end
        end
    end

    if options.accel
        if ~isfield(options, 'h_repeat')
            options.h_repeat = 1;
        else
        end

        if ~isfield(options, 'rep_mode')
            options.rep_mode = 'fix';
        else
        end
    else
        options.h_repeat = 1;
    end

    if options.h_repeat == 1
        if options.ls
            options.sub_mode = 'LS';
        else
            options.sub_mode = 'STD';
        end
    end

    if strcmp(options.rep_mode, 'adaptive')
        rhoh = 1+(m+m*rank)/(1*(rank+1));
        alpha = 2;
        delta = 0.01;
    end

    if ~isfield(options, 'x_init')
        Wt  = rand(m, rank);
        H   = rand(rank, n);
        R   = rand(m, n);
    else
        Wt  = options.x_init.W;
        H   = options.x_init.H;
        R   = options.x_init.R;
    end

    % permute samples
    if options.permute_on
        perm_idx = randperm(n);
    else
        perm_idx = 1:n;
    end
    V = V(:, perm_idx);
    H = H(:, perm_idx);
    R = R(:, perm_idx);

    if options.robust
        mode = 'R-SAGMU-NMF';
    else
        mode = 'SAGMU-NMF';
        R = zeros(m, n);
    end

    % initialize
    method_name = sprintf('%s (%s)', mode, options.sub_mode);
    epoch = 0;
    l = zeros(m, options.batch_size) + options.lambda;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % prepare arrays for vht and Whht
    vht = cell(number_of_batches,1);
    Whht = cell(number_of_batches,1);

    % store vht and Whht
    cnt = 0;
    for t=1: options.batch_size : n - 1
        cnt = cnt + 1;
        vt = V(:,t:t+options.batch_size-1);
        ht = H(:,t:t+options.batch_size-1);
        rt = R(:,t:t+options.batch_size-1);
        vht{cnt} = vt * ht';
        Whht{cnt} = (Wt * ht + rt) * ht';
    end

    % prepare Delta_minus and Delta_plus
    if options.fast_calc
        Delta_minus = zeros(m, rank);
        Delta_plus = zeros(m, rank);
    end

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, Wt, H, R, options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, options.sub_mode, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main outer loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        cnt = 0;
        % main inner loop
        for t = 1 : options.batch_size : n - 1
            cnt = cnt + 1;

            % retrieve vt and ht
            vt = V(:,t:t+options.batch_size-1);
            ht = H(:,t:t+options.batch_size-1);

            if ~options.robust

                % uddate ht
                Wtv = Wt' * vt;
                WtW = Wt' * Wt;
                if strcmp(options.sub_mode, 'ACC')
                    if strcmp(options.rep_mode, 'adaptive')
                        gamma = 1;
                        eps0 = 1;
                        j = 1;
                        rhoh_alpha = rhoh*alpha;
                        %while j <= floor(1+rhoh*alpha) &&  gamma >= delta*eps0
                        ht0 = ht;
                        while j <= rhoh_alpha && gamma >= delta*eps0
                            ht = ht .* (Wtv) ./ (WtW * ht);
                            ht = ht + (ht<eps) .* eps;
                            if j == 1
                                eps0 = norm(ht0-ht);
                            end
                            gamma = norm(ht0-ht);
                            j = j+1;
                        end
                    else
                        for iii=1:options.h_repeat
                            ht = ht .* (Wtv) ./ (WtW * ht);
                            ht = ht + (ht<eps) .* eps;
                        end
                        %ht = ht + (ht<eps) .* eps;
                    end
                elseif strcmp(options.sub_mode, 'LS')
                    ht = calc_nls_nmf(vt, Wt, 1e-16);
                    ht = ht + (ht<eps) * eps;
                else
                    ht = ht .* (Wtv) ./ (WtW * ht);
                    ht = ht + (ht<eps) .* eps;
                end
            else

                rt = R(:, t:t+options.batch_size-1);

                % uddate ht
                Wtv = Wt' * vt;
                %WtW = Wt' * Wt;
                if strcmp(options.sub_mode, 'ACC')
                    if strcmp(options.rep_mode, 'adaptive')
                        gamma = 1;
                        eps0 = 1;
                        j = 1;
                        rhoh_alpha = rhoh*alpha;
                        %while j <= floor(1+rhoh*alpha) &&  gamma >= delta*eps0
                        ht0 = ht;
                        while j <= rhoh_alpha && gamma >= delta*eps0
    %                         ht = ht .* (Wtv) ./ (Wt' * (Wt * ht + rt));
    %                         ht = ht + (ht<eps) .* eps;
    %                         rt = rt .* vt ./ (Wt * ht + rt + l);
                            Wh_r = Wt * ht + rt;
                            ht = ht .* (Wtv) ./ (Wt' * Wh_r);
                            ht = ht + (ht<eps) .* eps;
                            rt = rt .* vt ./ (Wh_r + l);
                            if j == 1
                                eps0 = norm(ht0-ht);
                            end
                            gamma = norm(ht0-ht);
                            j = j+1;
                        end
                    else
                        for iii=1:options.h_repeat
    %                         ht = ht .* (Wtv) ./ (Wt' * (Wt * ht + rt));
    %                         ht = ht + (ht<eps) .* eps;
    %                         rt = rt .* vt ./ (Wt * ht + rt + l);
                            Wh_r = Wt * ht + rt;
                            ht = ht .* (Wtv) ./ (Wt' * Wh_r);
                            ht = ht + (ht<eps) .* eps;
                            rt = rt .* vt ./ (Wh_r + l);
                        end
                    end
                else
    %                 ht = ht .* Wtv ./ (Wt' * (Wt * ht + rt));
    %                 ht = ht + (ht<eps) .* eps;
    %                 rt = rt .* vt ./ (Wt * ht + rt + l);
                    Wh_r = Wt * ht + rt;
                    ht = ht .* (Wtv) ./ (Wt' * Wh_r);
                    ht = ht + (ht<eps) .* eps;
                    rt = rt .* vt ./ (Wh_r + l);
                end

            end

            % store vht and Whht
            Whht_org = Whht{cnt};
            vht_org = vht{cnt};

            % update
            vht{cnt} = vt * ht';
            if ~options.robust
                Whht{cnt} = Wt * (ht * ht');
            else
                Whht{cnt} = (Wt * ht + rt) * ht';
            end

            if epoch > 0
                if options.fast_calc
                    delta_minus = Delta_minus/n + (vht{cnt} + Whht_org)/options.batch_size;
                    delta_plus = Delta_plus/n + (Whht{cnt} + vht_org)/options.batch_size;

                    % update Delta_minus and Delta_plus
                    Delta_minus = Delta_minus + (vht{cnt} - vht_org);
                    Delta_plus = Delta_plus + (Whht{cnt} - Whht_org);
                else
                    delta_minus = zeros(m, rank);
                    delta_plus = zeros(m, rank);

                    for jj=1:number_of_batches
                        delta_minus = delta_minus + vht{jj};
                        delta_plus = delta_plus + Whht{jj};
                    end

                    delta_minus = delta_minus/n;
                    delta_plus = delta_plus/n;

                    delta_minus = delta_minus + (vht{cnt} + Whht_org)/options.batch_size;
                    delta_plus = delta_plus + (Whht{cnt} + vht_org)/options.batch_size;
                end

            else
                delta_minus = vht{cnt}/options.batch_size;
                delta_plus = Whht{cnt}/options.batch_size;

                if options.fast_calc
                    % update Delta_minus and Delta_plus
                    Delta_minus = Delta_minus + vht{cnt};
                    Delta_plus = Delta_plus + Whht{cnt};
                end
            end

            % update W
            if options.stepsize_ratio == 1
                Wt = Wt .* (delta_minus ./ delta_plus);
            else
                Wt = (1-ratio)* Wt + ratio * Wt .* (delta_minus ./ delta_plus);
            end

            Wt = Wt + (Wt<eps) .* eps;

            % store new h
            H(:,t:t+options.batch_size-1) = ht;

            % update R
            if options.robust
                R(:,t:t+options.batch_size-1) = rt;
            end

            grad_calc_count = grad_calc_count + m * options.batch_size;
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, Wt, H, R, options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = Wt;
    x.H(:,perm_idx) = H;
    x.R = R;

end


function a = asin(a)
%ASIN         Gradient inverse sine asin(a)
%

% written  10/16/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 08/26/12     S.M. Rump  global variables removed
%

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

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  ax = 1 ./ sqrt( 1 - sqr(full(a.x(:))) );
  a.x = asin(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
      else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  if rndold
    setround(rndold)
  end


function x = cvx_c2r( x, dim, cleanup_eps )

%
% Determine expansion dimension
%

sx = size( x );
if nargin < 2,
    dim = [ find( sx > 1 ), 1 ];
    dim = dim( 1 );
elseif ~isnumeric( dim ) || dim <= 0 || dim ~= floor( dim ),
    error( 'Second argument must be a dimension.' );
end
sx = [ sx, ones( 1, dim - length( sx ) ) ];
nd = length( sx );

%
% Perform the sparse case differently
%

if isnumeric( x ) && issparse( x ) && dim <= 2,
    [ rr, cc, vv ] = find( x );
    vr = real( vv );
    vi = imag( vv );
    if nargin > 2,
        ndxs = find( vr & vi );
        if ~isempty( ndxs ),
            temp = abs( vr(ndxs) ./ vi(ndxs) );
            vr(ndxs(temp<=cleanup_eps)) = 0;
            vi(ndxs(temp>=1.0./cleanup_eps)) = 0;
        end
    end
    if dim == 1,
        rr = 2 * rr; rr = [ rr - 1 ; rr ];
        cc = [ cc ; cc ];
        sx( 1 ) = 2 * sx( 1 );
    else
        cc = 2 * cc; cc = [ cc - 1 ; cc ];
        rr = [ rr ; rr ];
        sx( 2 ) = 2 * sx( 2 );
    end
    x = sparse( rr, cc, [ vr ; vi ], sx( 1 ), sx( 2 ) );
    return
end

%
% Permute if necessary
%

perm = [];
if any( sx( 1 : dim - 1 ) ~= 1 ),
    perm = [ dim, 1 : dim - 1, dim + 1 : nd ];
    x = permute( x, perm );
    sx = sx( perm );
    dim = 1;
end

%
% Perform expansion and possibly cleanup
%

x = x( : ).';
sx( dim ) = 2 * sx( dim );
xr = real( x );
xi = imag( x );
if nargin > 2,
    ndxs = find( xr & xi );
    if ~isempty( ndxs ),
        temp = abs( xr(ndxs) ./ xi(ndxs) );
        xr(ndxs(temp<=cleanup_eps)) = 0;
        xi(ndxs(temp>=1.0./cleanup_eps)) = 0;
    end
end
x = reshape( [ xr ; xi ], sx );

%
% Reverse permute if necessary
%

if ~isempty( perm ),
    x = ipermute( x, perm );
end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function test_toolbox
%TEST_TOOLBOX Tests all functionalities of the dimension reduction toolbox
%
%   test_toolbox
%
% Tests all functionalities of the dimension reduction toolbox.
%
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    % Generate data
    disp('Testing data generation functions...');
    datasets = {'helix', 'twinpeaks', '3d_clusters', 'intersect', 'swiss'};
    for i=1:length(datasets)
        try
            X = generate_data(datasets{i}, 500);
        catch e
            disp(e);
            warning(['Generation of data set ' datasets{i} ' failed! Press any key to continue tests...']);
            pause
        end
    end

    % Test prewhitening
    disp('Testing prewhitening...');
    try
        X = prewhiten(X);
    catch e
        disp(e);
        warning('Prewhitening failed! Press any key to continue tests...');
        pause
    end
    unscaled_X = X;
    X = X - min(X(:));
    X = X / max(X(:));

    % Test all intrinsic dimensionality estimators
    disp('Testing intrinsic dimensionality estimators...');
    techniques = {'CorrDim', 'NearNbDim', 'GMST', 'PackingNumbers', 'EigValue', 'MLE'};
    for i=1:length(techniques)
        try
            intrinsic_dim(X, techniques{i});
        catch e
            disp(e);
            warning(['Intrinsic dimensionality estimation using ' techniques{i} ' failed! Press any key to continue tests...']);
            pause
        end
    end

    % Test all unsupervised dimension reduction techniques
    disp('Testing dimensionality reduction techniques...');
    techniques = {'PCA', 'MDS', 'ProbPCA', 'FactorAnalysis', 'GPLVM', 'Sammon', 'Isomap', ...
        'LandmarkIsomap', 'LLE', 'Laplacian', 'HessianLLE', 'LTSA', 'MVU', 'CCA', 'LandmarkMVU', ...
        'FastMVU', 'DiffusionMaps', 'KernelPCA', 'GDA', 'SNE', 'SymSNE', 'tSNE', 'LPP', 'NPE', ...
        'LLTSA', 'SPE', 'Autoencoder', 'LLC', 'ManifoldChart', 'CFA'};
    for i=1:length(techniques)

        % Test the dimension reduction technique
        try
            if any(strcmpi(techniques{i}, {'GPLVM', 'CFA'}))
                [mappedX, mapping] = compute_mapping(unscaled_X, techniques{i}, 2);
            else
                [mappedX, mapping] = compute_mapping(X, techniques{i}, 2);
            end
            if any(strcmpi(techniques{i}, {'Isomap', 'LandmarkIsomap', 'LLE', 'Laplacian', 'MVU', 'CCA', 'FastMVU', 'LPP', 'NPE', 'LLTSA'}))
                [mappedX, mapping] = compute_mapping(X, techniques{i}, 2, 'adaptive');
            end
        catch e
            disp(e);
            warning(['Technique ' techniques{i} ' failed! Press any key to continue tests...']);
            pause
        end

        % Test the out-of-sample extension code
        if any(strcmpi(techniques{i}, {'PCA', 'LPP', 'NPE', 'LLTSA', 'SPCA', 'PPCA', 'FA'}))
            try
                out_of_sample(X, mapping);
            catch e
                disp(e);
                warning(['Out-of-sample extension for technique ' techniques{i} ' failed! Press any key to continue tests...']);
                pause
            end
        end

        % Test reconstruction code
        if any(strcmpi(techniques{i}, {'PCA', 'LPP', 'NPE', 'LLTSA', 'SPCA', 'PPCA', 'FA', 'Autoencoder'}))
            try
                reconstruct_data(mappedX, mapping);
            catch e
                disp(e);
                warning(['Reconstruction for technique ' techniques{i} ' failed! Press any key to continue tests...']);
                pause
            end
        end
    end

    % Test approximate out-of-sample function
    try
        out_of_sample_est(X, X, mappedX);
    catch e
        disp(e);
        warning(['Approximate out-of-sample extension failed! Press any key to continue tests...']);
        pause
    end

    % Test all supervised dimension reduction techniques
    labels = double(X(:,1) > .5) + 1;
    X = [labels X];
    techniques = {'LDA', 'NCA', 'MCML', 'LMNN'};
    for i=1:length(techniques)

        % Test the actual technique
        try
            [mappedX, mapping] = compute_mapping(X, techniques{i}, 2);
        catch e
            disp(e);
            warning(['Technique ' techniques{i} ' failed! Press any key to continue tests...']);
            pause
        end

        % Test out-of-sample extension
        try
            out_of_sample(X(:,2:end), mapping);
        catch e
            disp(e);
            warning(['Out-of-sample extension for technique ' techniques{i} ' failed! Press any key to continue tests...']);
            pause
        end

        % Test reconstruction code
        try
            reconstruct_data(mappedX, mapping);
        catch e
            disp(e);
            warning(['Reconstruction for technique ' techniques{i} ' failed! Press any key to continue tests...']);
            pause
        end

    end
    disp('All tests completed!');


function s = obj2segs(obj)

% OBJ2SEGS Get segments matrix from 3D object.
%   OBJ2SEGS(OBJ) returns a 3-by-N matrix of segments from the 3D
%   information in structure OBJ, stored in the N-by-3 matrix OBJ.vert,
%   with the object vertices, and the M-by-2 matrix OBJ.seg, with the
%   couples of indices in OBJ.vert defining each segment.
%
%   For example, the square:
%
%     1 +-----+ 2
%       |     |
%       |     |
%     3 +-----+ 4
%
%   is defined in OBJ as
%       OBJ.vert = [0 0 0; 1 0 0; 0 1 0; 1 1 0]
%       OBJ.seg  = [1 2; 2 4; 4 3; 3 1]
%   then the produced segments matrix SEG = OBJ2SEGS(OBJ) is
%       SEG = [ 0 1 1 0
%               0 0 1 1
%               0 0 0 0
%               1 1 0 0
%               0 1 1 0
%               0 0 0 0 ]

%   Copyright 2009 Joan Sola @ LAAS-CNRS.

p = obj2pnts(obj);    % endpoints in 3-by-N format
M = size(obj.seg,1);  % number of segments M
s = zeros(6,M);       % initialize output segments matrix

for i = 1:M  % for each segment ...

    j = obj.seg(i,1); % first endpoint's index
    k = obj.seg(i,2); % second endpoint's index
    s(:,i) = [p(:,j);p(:,k)]; % the segment!

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [post,nlZ,dnlZ] = infExact_fastrobust(kmax, hyp, mean, cov, lik, x, y, s)

% Fast, robust inference for a GP with Gaussian likelihood. Compute a
% parametrization of the posterior, the negative log marginal likelihood
% and its derivatives w.r.t. the hyperparameters. See also "help infMethods".
%
% To be used in combination with 'fast' covariance functions (e.g.
% covSEard_fast and such).
%
% This implementation also handles badly conditioned covariance matrices,
% which sometimes become non-positive definite and cause a failure of the
% Cholesky decomposition. In these cases, the covariance matrix is corrected
% to the nearest symmetric positive semidefinite (SPD) matrix in Frobenius
% norm (see [1]; based on an implementation by John d'Errico).
%
% [1] Higham NJ, "Computing a nearest symmetric positive semidefinite
% matrix", Linear Algebra Appl, 1988.
% http://www.sciencedirect.com/science/article/pii/0024379588902236
%
% Added support for 'fast' inference and robust Cholesky decomposition by
% Luigi Acerbi, 2016-01-03.
% Original code by Carl Edward Rasmussen and Hannes Nickisch, 2015-07-13.
%
% See also INFMETHODS.M.

if iscell(lik), likstr = lik{1}; else likstr = lik; end
if ~ischar(likstr), likstr = func2str(likstr); end
if strcmp(likstr,'likGauss')    % NOTE: no explicit call to likGauss
    henoise_flag = false;
elseif strcmp(likstr,'likGaussHe')
    henoise_flag = true;        % Heteroskedastic noise
else
    error('Exact inference only possible with Gaussian likelihood');
end

if isempty(kmax); kmax = 5; end

[n, D] = size(x);
if nargout > 2                                      % do we want derivatives?
  [K,dK] = feval(cov{:}, hyp.cov, x);                 % evaluate covariance matrix and derivatives
else
  K = feval(cov{:}, hyp.cov, x);                      % evaluate covariance matrix
end
m = feval(mean{:}, hyp.mean, x);                          % evaluate mean vector

sn2_base = exp(2*hyp.lik);                               % noise variance of likGauss

if henoise_flag && ~isempty(s)
    % if isempty(s); error('Input-dependent vector S is empty.'); end
    sn2 = sn2_base + s.^2;                               % Vector of observation variance
else
    sn2 = sn2_base;
end

Lchol = min(sn2) >= 1e-6;       % tiny sn2 can lead to numerical trouble

% smallnoise = sn2 < 1e-6;
if Lchol
    if isscalar(sn2)
        sn2div = sn2;
        sn2_mat = eye(n);
    else
        sn2div = min(sn2);
        sn2_mat = diag(sn2/sn2div);
    end

    M = K/sn2div+sn2_mat;       % B matrix
else
    if isscalar(sn2)
        sn2_mat = sn2*eye(n);
    else
        sn2_mat = diag(sn2);
    end
    M = K+sn2_mat;           % Covariance with noise
end

[L,p] = chol(M);                % Try computing Cholesky factor

if p~=0     % Failed Cholesky decomposition, compute nearest SPD matrix
    if kmax <= 0; error('Cannot compute Cholesky decomposition.'); end
    % Mold = M;

    M = (M + M')/2;         % Ensure M is symmetric
    [U,Sigma,V] = svd(M);
    H = V*Sigma*V';         % Symmetric polar factor H is SPD
    M = (M+H)/2;
    M = (M + M')/2;         % Ensure symmetry again
    [L,p] = chol(M);        % Retry Cholesky decomposition
    k = 0;
    while p ~= 0            % Failed again, add a small diagonal component
        k = k + 1;
        % We do not want to change the matrix too much
        if k > kmax-1; error('Cannot compute Cholesky decomposition.'); end
        lambda = eig(M);
        mineig = min(lambda);
        kappa = 0.05;        % Sometimes even a small nudge is sufficient
        M = M + kappa*abs(mineig)*k.^2*eye(size(M));
        [L,p] = chol(M);
    end

    % [sort(eig(M))'; sort(eig(Mold))']

end

if Lchol
    sl = sn2div;
    pL = L;                         % L = chol(eye(n)+sW*sW'.*K)
else
    sl = 1;
    pL = -solve_chol(L,eye(n));     % L = -inv(K+inv(sW^2))
end

alpha = solve_chol(L,y-m)/sl;

post.alpha = alpha;                            % return the posterior parameters
post.sW = ones(n,1)/sqrt(min(sn2));            % sqrt of noise precision vector
post.L = pL;
post.Lchol = Lchol;

if nargout>1                               % do we want the marginal likelihood?
  nlZ = (y-m)'*alpha/2 + sum(log(diag(L))) + n*log(2*pi*sl)/2;   % -log marg lik
  if nargout>2                                         % do we want derivatives?
    dnlZ = hyp;                                 % allocate space for derivatives
    Q = solve_chol(L,eye(n))/sl - alpha*alpha';     % precompute for convenience
    for i = 1:numel(hyp.cov)
      dnlZ.cov(i) = sum(sum(Q.*dK(:,:,i)))/2;
    end
    dnlZ.lik = sn2_base*trace(Q);
    for i = 1:numel(hyp.mean)
      dnlZ.mean(i) = -feval(mean{:}, hyp.mean, x, i)'*alpha;
    end
  end
end


function featAugm = get_augm_spatial_features_same_regr(feat)

% relative coord of 2 detections
deltaX = feat(:, 1);
deltaY = feat(:, 2);

dist_sq = deltaX.^2 +deltaY.^2;
dist = sqrt(dist_sq);

featAugm = cat(2, dist, dist_sq);
end

function angle = compute_angle(deltaX, deltaY)
angle = atan2(deltaY,deltaX);
angle = wrapMinusPiPifast(angle);
assert(all(angle <= pi) && all(angle >= -pi));
end

function a = wrap_angle(a)
larger = a > pi;
smaller = a < -pi;
a(larger)  = a(larger) - 2*pi;
a(smaller) = a(smaller)+ 2*pi;
end


clear; close all; clc;

%cfig = figure('Position', [10,10,1280,1080]);
cfig = figure(1);

% Lidar parameters
lidar = SetLidarParameters();

% Map parameters
borderSize      = 1;            % m
pixelSize       = 0.2;          % m
miniUpdated     = false;        %
miniUpdateDT    = 0.1;          % m
miniUpdateDR    = deg2rad(5);   % rad
% If the robot has moved 0.1 m or rotated 5 degree from last key scan,
% we would add a new key scan and update the map

% Scan matching parameters
fastResolution  = [0.05; 0.05; deg2rad(0.5)]; % [m; m; rad]
bruteResolution = [0.01; 0.01; deg2rad(0.1)]; % not used

% Load lidar data
lidar_data = load('dataset/horizental_lidar.mat');
N = size(lidar_data.timestamps, 1);

% Create an empty map
map.points = [];
map.connections = [];
map.keyscans = [];
pose = [0; 0; 0];
path = pose;

% Here we go!!!!!!!!!!!!!!!!!!!!
for scanIdx = 1 : 1 : N

    disp(['scan ', num2str(scanIdx)]);

    % Get current scan [x1,y1; x2,y2; ...]
    time = lidar_data.timestamps(scanIdx) * 1e-9;
    scan = ReadAScan(lidar_data, scanIdx, lidar, 24);

    % If it's the first scan, initiate
    if scanIdx == 1
        map = Initialize(map, pose, scan);
        miniUpdated = true;
        continue;
    end

    % ===== Matching current scan to local map ============
    % 1. If we executed a mini update in last step, we shall update the
    %    local points map and local grid map (coarse)
    if miniUpdated
        localMap = ExtractLocalMap(map.points, pose, scan, borderSize);
        gridMap1 = OccuGrid(localMap, pixelSize);
        gridMap2 = OccuGrid(localMap, pixelSize/2);
    end

    % 2. Predict current pose using constant velocity motion model
    if scanIdx > 2
        pose_guess = pose + DiffPose(path(:,end-1), pose);
    else
        pose_guess = pose;
    end

    % 3. Fast matching
    if miniUpdated
        [pose, ~] = FastMatch(gridMap1, scan, pose_guess, fastResolution);
    else
        [pose, ~] = FastMatch(gridMap2, scan, pose_guess, fastResolution);
    end

    % 4. Refine the pose using smaller pixels
    % gridMap = OccuGrid(localMap, pixelSize/2);
    [pose, hits] = FastMatch(gridMap2, scan, pose, fastResolution/2);
    %----------------------------------------------------------------------

    % Execute a mini update, if the robot has moved a certain distance
    dp = abs(DiffPose(map.keyscans(end).pose, pose));
    if dp(1)>miniUpdateDT || dp(2)>miniUpdateDT || dp(3)>miniUpdateDR
        miniUpdated = true;
        [map, pose] = AddAKeyScan(map, gridMap2, scan, pose, hits,...
                        pixelSize, bruteResolution, 0.1, deg2rad(3));
    else
        miniUpdated = false;
    end
    path = [path, pose];

    % ===== Loop Closing =========================================
%     if miniUpdated
%         if TryLoopOrNot(map)
%             map.keyscans(end).loopTried = true;
%             map = DetectLoopClosure(map, scan, hits, 4, pi/6, pixelSize);
%         end
%     end

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

    % Plot
    if mod(scanIdx, 30) == 0
        PlotMap(cfig, map, path, scan, scanIdx);
    end

end


function [neighbors] = BuildNonCon2D(NGauss, tol)

% function [neighbors] = BuildNonCon2D(NGauss, tol)
% Purpose: find element to element connections through non-conforming interfaces
%          (** elements assumed straight sided **)

Globals2D;

% 1. Build Gauss nodes
[gz, gw] = JacobiGQ(0, 0, NGauss-1);

% 1.1 Find location of vertices of boundary faces
vx1 = VX(EToV(:,[1,2,3])); vx2 = VX(EToV(:,[2,3,1]));
vy1 = VY(EToV(:,[1,2,3])); vy2 = VY(EToV(:,[2,3,1]));

idB = find(EToE==((1:K)'*ones(1,Nfaces)));
x1 = vx1(idB)'; y1 = vy1(idB)';
x2 = vx2(idB)'; y2 = vy2(idB)';

% 1.2 Find those element-faces that are on boundary faces
[elmtsB,facesB] = find(EToE==((1:K)'*ones(1,Nfaces)));
Nbc = length(elmtsB);

sk = 1;
% 2.1 For each boundary face
for b1=1:Nbc
  % 2.2 Find element and face of this boundary face
  k1 = elmtsB(b1); f1 = facesB(b1);

  % 2.3 Find end coordinates of b1'th boundary face
  x11 = x1(b1);  y11 = y1(b1);  x12 = x2(b1);  y12 = y2(b1);

  % 2.4 Compute areas, lengths and face coordinates used in intersection
  % tests comparing b1'th boundary face with all boundary faces
  area1 = abs((x12-x11)*(y1-y11) - (y12-y11)*(x1-x11)); %scale
  area2 = abs((x12-x11)*(y2-y11) - (y12-y11)*(x2-x11));
  L   = (x12-x11)^2 + (y12-y11)^2 ;
  r21 = ((2*x1-x11-x12)*(x12-x11) + (2*y1-y11-y12)*(y12-y11))/L;
  r22 = ((2*x2-x11-x12)*(x12-x11) + (2*y2-y11-y12)*(y12-y11))/L;

  % 2.5 Find range of local face coordinate (bracketed between -1 and 1)
  r1 = max(-1,min(r21,r22)); r2 = min(1,max(r21,r22));

  % 2.6 Compute flag for overlap of b1 face with all other boundary faces
  flag = area1+area2+(r1<= -1 & r2<= -1)+(r1>=1 & r2>=1)+(r2-r1<tol);

  % 2.7 Find other faces with partial matches
  matches = setdiff(find(flag < tol),b1);
  Nmatches = length(matches(:));

  if(Nmatches>0)
    % 3.1 Find matches
    r1 = r1(matches); r2 = r2(matches);

    % 3.2 Find end points of boundary-boundary intersections
    xy11 = 0.5*[x11;y11]*(1-r1) +  0.5*[x12;y12]*(1+r1);
    xy12 = 0.5*[x11;y11]*(1-r2) +  0.5*[x12;y12]*(1+r2);

    % 3.3 For each face-face match
    for n=1:Nmatches

      % 3.4 Store which elements intersect
      k2 = elmtsB(matches(n)); f2 = facesB(matches(n));
      neighbors{sk}.elmtM = k1; neighbors{sk}.faceM = f1;
      neighbors{sk}.elmtP = k2; neighbors{sk}.faceP = f2;

      % 3.5 Build physical Gauss nodes on face fragment
      xg = 0.5*(1-gz)*xy11(1,n) + 0.5*(1+gz)*xy12(1,n);
      yg = 0.5*(1-gz)*xy11(2,n) + 0.5*(1+gz)*xy12(2,n);

      % 3.6 Find local coordinates of Gauss nodes
      [rg1,sg1] = FindLocalCoords2D(k1, xg, yg);
      [rg2,sg2] = FindLocalCoords2D(k2, xg, yg);

      % 3.7 Build interpolation matrices for volume nodes ->Gauss nodes
      gVM = InterpMatrix2D(rg1,sg1); neighbors{sk}.gVM  = gVM;
      gVP = InterpMatrix2D(rg2,sg2); neighbors{sk}.gVP  = gVP;

      % 3.8 Find face normal
      neighbors{sk}.nx = nx(1+(f1-1)*Nfp,k1);
      neighbors{sk}.ny = ny(1+(f1-1)*Nfp,k1);

      % 4.0 Build partial face data lift operator

      % 4.1 Compute weights for lifting
      partsJ = sqrt( (xy11(1,n)-xy12(1,n))^2 + (xy11(2,n)-xy12(2,n))^2 )/2;
      dgw = gw*partsJ/J(1,k1);

      % 4.2 Build matrix to lift Gauss data to volume data
      neighbors{sk}.lift = V*V'*(gVM')*diag(dgw);

      sk = sk+1;
    end
  end
end
return


function [theta] =  tapas_sem_multiv_init_theta(data, model, inference)
%% Obtain an initial sample with positive likelihood.
%
% aponteeduardo@gmail.com
% copyright (C) 2017
%

ptheta = model.graph{1}.htheta.model;

nc = size(model.graph{1}.htheta.T, 2);
ns = size(data, 1);
mu = model.graph{4}.htheta.y.mu;
nb = numel(mu);
np = size(ptheta.jm, 2);

ty = ptheta.x * model.graph{4}.htheta.y.mu;
ty = reshape(ty', numel(ty), 1);
theta = repmat(mat2cell(ty, np * ones(ns, 1), 1), 1, 1);

llh = tapas_sem_multiv_llh(data, struct('y', {theta}), model.graph{1}.htheta);
failing = find(llh == -inf);

tolerance = 1000;
while numel(failing) && tolerance
    for i = failing'
        theta{i} = sample_gaussian(ptheta);
    end
    llh = tapas_sem_multiv_llh(data, struct('y', {theta}), ...
        model.graph{1}.htheta);
    failing = find(llh == -inf);
    tolerance = tolerance - 1;
end

if numel(failing)
    error('tapas:sem:multiv:init', ...
    'It was not possible to initialize a sample with positive likelihood');
end

theta = repmat(theta, 1, nc);

end

function [theta] = sample_gaussian(ptheta)
%% Sample from a Gaussian prior.
%
% aponteeduardo@gmail.com
% copyright (C) 2016
%

np = size(ptheta.jm, 2);

sample_pars = logical(sum(ptheta.jm, 2));

if size(ptheta.pm, 2) ~= np
    pm = diag(ptheta.pm);
else
    pm = ptheta.pm;
end

lt = chol(pm);
theta = lt \ ptheta.jm * randn(np, 1);
theta = ptheta.sm' * theta;

end


function imgOut = WardHistAdjTMO(img, nBin, LdMin, LdMax, bPlotHistogram, bDownsampling)
%
%        imgOut = WardHistAdjTMO(img, nBin, LdMin, LdMax, bPlotHistogram, bDownsampling)
%
%
%        Input:
%           -img: input HDR image
%           -nBin: number of bins for calculating the histogram (1,+Inf)
%           -LdMin: minimum luminance value of the dispLay
%           -LdMax: maximum luminance value of the dispLay
%           -bPlotHistogram:
%
%        Output:
%           -imgOut: tone mapped image
%
%     Copyright (C) 2010-21  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%     The paper describing this technique is:
%     "A Visibility Matching Tone Reproduction Operator for High Dynamic Range Scenes"
% 	  by Gregory Ward Larson, Holly Rushmeier, Christine Piatko
%     in IEEE Transactions on Visualization and Computer Graphics 1997
%

%is it a gray/three color channels image?
check13Color(img);

checkNegative(img);

if(~exist('nBin', 'var'))
    nBin = 100;
end

if(nBin < 1)
    nBin = 100;
end

if(~exist('LdMin', 'var'))
    LdMin = 1; %cd/m^2
end

if(LdMin < 0.0)
    LdMin = 1;
end

if(~exist('LdMax', 'var'))
    LdMax = 100; %cd/m^2
end

if(LdMax <= 0.0)
    LdMax = 100;
end

if(LdMax < LdMin)
    tmp = LdMin;
    LdMin = LdMax;
    LdMax = tmp;
end

if(~exist('bPlotHistogram', 'var'))
    bPlotHistogram = 0;
end

if(~exist('bDownsampling', 'var'))
    bDownsampling = 0;
end

epsilon = 1e-6;

%compute luminance channel
L = lum(img);

%downsample according to fovea...
if(bDownsampling)
    L2 = WardDownsampling(L + epsilon);
else
    L2 = L + 1e-6;
end

%compute stastistics
LMin = min(L2(:));
LMax = max(L2(:));

Llog  = log(L2);

LlMin = log(LMin);
LlMax = log(LMax);

LldMin = log(LdMin + epsilon);
LldMax = log(LdMax + epsilon);

%compute the histogram H
H = zeros(nBin, 1);
delta = (LlMax - LlMin) / nBin;

for i=1:nBin
    indx = find(Llog > (delta * (i - 1) + LlMin) & Llog <= (delta * i + LlMin));
    H(i) = numel(indx);
end

%apply the histogram ceiling
maxH = max(H);
x_vis = LlMin:((LlMax - LlMin) / (nBin -1)):LlMax;

if(bPlotHistogram)
    bar(x_vis, H/maxH);
    hold on;
end

H = histogram_ceiling(H, delta / (LldMax - LldMin));

if(bPlotHistogram)
    bar(x_vis, H / maxH);
    hold off;
end

%compute P(x)
P = cumsum(H);
P = P / max(P);

%calculate tone mapped luminance
L(L > LMax) = LMax;
x = (LlMin:((LlMax - LlMin) / (nBin - 1)):LlMax)';
P_L = interp1(x , P , real(log(L)), 'linear');
Ld  = exp(LldMin + (LldMax - LldMin) * P_L);
%normalize in [0,1]
Ld  = (Ld - LdMin) / (LdMax - LdMin);

%change luminance
imgOut = ChangeLuminance(img, L, Ld);
end


function snirfSegment()
% Description: This function divides the nirs data into segments predefined by
% user input. User should input the time period of the segments of the nirs
% data as eg [0 200; 400:900; 950:1000] in *sec*
% Meryem A Yucel, Nov 2016
%%%%%%%%%%%%%%%
% Code refined for Homer3 and fNIRS files

% Homer3 Note
% Segment tool does not work for a flat file structure

global maingui;
if isempty(maingui)
    [files, pathnm] = uigetfile( '*.snirf', 'Pick the .snirf file', 'multiselect','on');
else
    fpath = [maingui.dataTree.currElem.path maingui.dataTree.currElem.name];
    [files, pathnm] = uigetfile( '*.snirf', 'Pick the .snirf file', fpath, 'multiselect','on');
end

if files==0
    return
end

if ~iscell(files)
    if files==0
        return;
    end
end

[~,name,~] = fileparts(files);

fsn = inputdlg( 'Select the time range (in seconds) for the segment of data to save in a separate file. You can enter multiple time ranges, separated by a '';'', to save different segments to different files. For example, [0 100; 300 400] would give you two segments of the original file from 0 to 100 and 300 to 400', 'Segment SNIRF file', 1 );
if isempty(fsn)
    return
end
fsn = str2num(fsn{1});

wd = cd;
cd(pathnm)

if ~iscell(files)
    foo{1} = files;
    files = foo;
end

for iFile = 1:length(files)
    snirfData = SnirfClass(files{iFile});
    fs = 1/(snirfData.data.time(2) - snirfData.data.time(1));
    maxT = max(snirfData.data.time);

    if fsn(1,1) == 0 % if time starts from 0, take the first data sample!
        fsn(1,1) = 1/fs;
    end

    for P = 1:size(fsn,1) % loop over different parts
        snirfData = SnirfClass(files{iFile});
        if fsn(P,1)> maxT || fsn(P,2) > maxT
            errordlg('Time period (in sec) exceeds the maximum time. Please re-try.','Retry');
            return
        elseif fsn(P,1) < 0 || fsn(P,2) < 0
            errordlg('Time period (in sec) should consist of positive numbers. Please re-try.','Retry');
            return
        end

        snirfData.data.dataTimeSeries = snirfData.data.dataTimeSeries(round(fsn(P,1)*fs):round(fsn(P,2)*fs),:);
        snirfData.data.time = snirfData.data.time(round(fsn(P,1)*fs):round(fsn(P,2)*fs));
        for iStim = 1:length(snirfData.stim)
            if ~isempty(snirfData.stim(iStim).data)
                snirfData.stim(iStim).data = snirfData.stim(iStim).data(snirfData.stim(iStim).data(:,1)>fsn(P,1) & snirfData.stim(iStim).data(:,1)<fsn(P,2),:);
            end
        end
        for iAux = 1:length(snirfData.aux)
            snirfData.aux(iAux).dataTimeSeries = snirfData.aux(iAux).dataTimeSeries(round(fsn(P,1)*fs):round(fsn(P,2)*fs));
            snirfData.aux(iAux).time = snirfData.aux(iAux).time(round(fsn(P,1)*fs):round(fsn(P,2)*fs));
        end

        snirfName = sprintf([name '_seg_' num2str(P) '.snirf']);
        snirfData.Save(snirfName);
        msgbox(['File created with name' snirfName], 'Notification');
    end
end

new_name = [files{1} '.orig'];
movefile (files{1}, new_name);
% if isempty(maingui)
%     %Nothing
% else
%     for iFile = 1:length(files)
%
%         maingui.dataTree.
%     end
% end

cd(wd);


clc,clear all,close all;
[GaitDBDir,probes,numprobe]=testData;
disp(['Processing ',GaitDBDir]);

maxDim=600;
MPCADADim=200;%Dimension of MPCA features b4 LDA
Qval=97;%The Q value, percentage of energy kept
maxK=1;%Maximum number of iterations
strGRDB=[GaitDBDir(12:(end-1))];
%%%%%%%%%%Load gallery gait data
setname='Gal';
load([GaitDBDir,setname]);
Is=size(fea3D);numSpl=Is(4);

fea3D_Train = fea3D;gnd_Train = gnd;clear fea3D
%=================MPCALDA==========================
disp(['Training on gallery']);
[tUs, posIdx, TXmean, Wgt, LDAU] = MPCALDA(fea3D_Train,gnd_Train,MPCADADim,Qval,maxK);
if size(LDAU,2)<maxDim,maxDim=size(LDAU,2);end
testDims=21:maxDim;

newfea=ttm(tensor(fea3D_Train-repmat(TXmean,[ones(1,3), numSpl])),tUs,1:3);
clear fea3D_Train
newfea = reshape(newfea.data,size(newfea,1)*size(newfea,2)*size(newfea,3),numSpl)';
galfea = newfea(:,posIdx);
galfea=galfea*LDAU;
galgnd=gnd;

nDim=length(testDims);
AllSeqR1s=zeros(nDim,numprobe);%Rank 1 recognition rate based on matching gait sequences
AllSeqR5s=zeros(nDim,numprobe);%Rank 5 recognition rate based on matching gait sequences
for iprb=1:numprobe
    %%%%%%%%%%Load probe gait data
    setname=['Prb',probes(iprb)];
    disp(['Testing ',setname]);
    load([GaitDBDir,setname]);Is=size(fea3D);numSpl=Is(4);
    newfea=ttm(tensor(fea3D-repmat(TXmean,[ones(1,3), numSpl])),tUs,1:3);clear fea3D
    newfea = reshape(newfea.data,size(newfea,1)*size(newfea,2)*size(newfea,3),numSpl)';
    prbfea = newfea(:,posIdx);
    prbfea=prbfea*LDAU;
    prbgnd=gnd;
    [SeqR1s,SeqR5s]=MADAll(galfea,galgnd,prbfea,prbgnd,testDims,Wgt);
    AllSeqR1s(:,iprb)=SeqR1s;
    AllSeqR5s(:,iprb)=SeqR5s;
end
MaxSeqR1=max(AllSeqR1s);
MaxSeqR5=max(AllSeqR5s);
disp('Rank 1 identification rate for Probe A to G')
disp(MaxSeqR1)
disp('Rank 5 identification rate for Probe A to G')
disp(MaxSeqR5)
disp(['Mean rank 1 identification rate=',num2str(mean(MaxSeqR1))])
disp(['Mean rank 5 identification rate=',num2str(mean(MaxSeqR5))])


function xhpf = HPF(u)
%
%
persistent prevX
persistent prevU
persistent dt tau
persistent firstRun

if isempty(firstRun)
  prevX = 0;
  prevU = 0;
  dt    = 0.01;
  tau   = 0.0233;

  firstRun = 1;
end

alpha = tau / (tau + dt);
xhpf = alpha*prevX + alpha*(u - prevU);

prevX = xhpf;
prevU = u;


% ------------------------------------------------------------------------
%  Copyright (C)
%  Universitat Politecnica de Catalunya BarcelonaTech (UPC) - Spain
%  University of California Berkeley (UCB) - USA
%
%  Jordi Pont-Tuset <jordi.pont@upc.edu>
%  Pablo Arbelaez <arbelaez@berkeley.edu>
%  June 2014
% ------------------------------------------------------------------------
% This file is part of the MCG package presented in:
%    Arbelaez P, Pont-Tuset J, Barron J, Marques F, Malik J,
%    "Multiscale Combinatorial Grouping,"
%    Computer Vision and Pattern Recognition (CVPR) 2014.
% Please consider citing the paper if you use this code.
% ------------------------------------------------------------------------
function sPb_thin = spectralPb_fast(ws_wt2, nvec, ic_gamma, dthresh)
% function sPb_thin = spectralPb_fast(ws_wt2, nvec, ic_gamma, dthresh)
%
% description:
%   fast spectral gradient contours
%
% Jon Barron and Pablo Arbelaez
% <arbelaez@berkeley.edu>
% Jan 2014

if nargin<4, dthresh = 2; end
if nargin<3, ic_gamma = 0.12; end
if nargin<2, nvec = 6; end

[tx2, ty2] = size(ws_wt2);
tx=(tx2-1)/2; ty=(ty2-1)/2;

l{1} = ws_wt2(1:2:end,2:2:end);
l{2}= ws_wt2(2:2:end,1:2:end);

% build the pairwise affinity matrix
[val,I,J] = buildW(l{1},l{2}, dthresh, ic_gamma);
W = sparse(val,I,J);

[EigVect, EVal] =  ncuts_downsample3(W, nvec, 2, 2, [ty, tx]);

clear D W opts;

EigVal = diag(EVal);
clear Eval;

EigVal(1:end) = EigVal(end:-1:1);
EigVect(:, 1:end) = EigVect(:, end:-1:1);

vect = zeros(tx, ty, nvec);
for v = 2 : nvec,
    vect(:, :, v) = reshape(EigVect(:, v), [ty tx])';
end
clear EigVect;

%% spectral Pb
for v=2:nvec,
    vect(:,:,v)=(vect(:,:,v)-min(min(vect(:,:,v))))/(max(max(vect(:,:,v)))-min(min(vect(:,:,v))));
end

sPb_thin = zeros(2*tx+1, 2*ty+1);
for v = 1 : nvec
    if EigVal(v) > 0,
        vec = vect(:,:,v)/sqrt(EigVal(v));
        sPb_thin = sPb_thin + seg2bdry_wt(vec, 'doubleSize');
     end
end
sPb_thin = sPb_thin.^(1/sqrt(2));

%%

function [EV, EVal] = ncuts_downsample3(A, NVEC, N_DOWNSAMPLE, DECIMATE, SZ)
% A = affinity matrix
% NEVC = number of eigenvectors (set to 16?)
% N_DOWNSAMPLE = number of downsampling operations (2 seems okay)
% DECIMATE = amount of decimation for each downsampling operation (set to 2)
% SZ = size of the image corresponding to A

A_down = A;
SZ_down = SZ;

Bs = cell(N_DOWNSAMPLE,1);
for di = 1:N_DOWNSAMPLE

    % Create a binary array of the pixels that will remain after decimating
    % every other row and column
    [i,j] = ind2sub(SZ_down, 1:size(A_down,1));
    do_keep = (mod(i, DECIMATE) == 0) & (mod(j, DECIMATE) == 0);

    % Downsample the affinity matrix
    A_sub = A_down(:,do_keep)';

    % Normalize the downsampled affinity matrix
    d = (sum(A_sub,1) + eps);
    B = bsxfun(@rdivide, A_sub, d)';

    % "Square" the affinity matrix, while downsampling
    A_down = A_sub*B;
    SZ_down = floor(SZ_down / 2);

    % Hold onto the normalized affinity matrix for bookkeeping
    Bs{di} = B;
end

% Get the eigenvectors of the Laplacian
%EV = ncuts(A_down, NVEC);
[EV, EVal] = ncuts2(A_down, NVEC);

% diag(EVal)

% Upsample the eigenvectors
for di = N_DOWNSAMPLE:-1:1
    EV = Bs{di} * EV;
end

% whiten the eigenvectors, as they can get scaled weirdly during upsampling
EV = whiten(EV, 1, 0);

function [EV, EVal] = ncuts2(A, n_ev)
[wx, wy] = size(A);
x = 1 : wx;
S = full(sum(A, 1));
D = sparse(x, x, S, wx, wy);
clear S x;

opts.issym=1;
opts.isreal = 1;
opts.disp=0;
[EV, EVal] = eigs((D - A) + (10^-10) * speye(size(D)), D, n_ev, 'sm', opts);
clear D A opts;

v = diag(EVal);
[sorted, sortidx] = sort(v, 'descend');
EV = EV(:,sortidx);
EVal = diag(sorted);


function score = BIC_score_CPD(CPD, fam, data, ns, cnodes)
% BIC_score_CPD Compute the BIC score of a tabular CPD
% score = BIC_score_CPD(CPD, fam, data, ns, cnodes)

if iscell(data)
  local_data = cell2num(data(fam,:));
else
  local_data = data(fam, :);
end
counts = compute_counts(local_data, CPD.sizes);
CPT = mk_stochastic(counts); % MLE
tiny = exp(-700);
CPT = CPT + (CPT==0)*tiny;  % replace 0s by tiny
LL = sum(log(CPT(:)) .* counts(:));
N = size(data, 2);
score = LL - 0.5*CPD.nparams*log(N);


function [u,p,info,Ai,Bi,invMt,Pro_u,Pro_p] = mgstokesRT0(A,B,f,g,u,p,node,elem,ufreeDof,option,varargin)
%% MGSTOKES
%
% Created by Ming Wang and Jie Zhou based on discussion with Long Chen.
% Improvement by Long Chen: improve efficiency, include smoother and add
% bisection case and BDM1B.
%
% See also: asmgstokes, mgstokes, mg, mgMaxwell
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

tic;

% Assign default values to unspecified parameters
if ~exist('option','var')
    option = [];
end
inputoption = option; % record the input one
option = mgoptions(option,length(f)+length(g));    % parameters
% specific choices for mgstokesRT0
if ~isfield(inputoption,'smoothingstep')  % smoothing steps
    option.smoothingstep = 2;
end
tol = option.tol; maxIt = option.solvermaxit;
smoothingstep = option.smoothingstep; solver = option.solver;
printlevel = option.printlevel; setupflag = option.setupflag;

if setupflag == true
%% Reconstruced hierarchical meshes and transfer operators
N0 = 8;
N = size(node,1);
% NT = size(elem,1);
level = 20;
NL(level+1) = N; % now NL(1:level) = 0;
nodei = cell(level,1);
elemi = cell(level,1);
Pro_u = cell(level,1);
Pro_p = cell(level,1);
nodei{level} = node;
elemi{level} = elem;
for j = level: -1 : 2
    switch option.refType
        case 'red'
            [elemi{j-1},newHB] = uniformcoarsenred(elemi{j}); % coasen red refinement
            if ~isempty(newHB)
                Pro_u{j-1} = transferedgered(elemi{j-1},elemi{j}); % transfer operator of u
                Pro_p{j-1} = repmat(speye(size(elemi{j-1},1)),4,1);   % transfer operator of p
            end
        case 'bisect'
            % merge two coarsen of bisection grids s.t. the ratio is 1/4.
            [tempelem,newHB1,tree] = uniformcoarsen(elemi{j}); % coarse bisection
            if ~isempty(newHB1)
                % first coarsen
                tempPro_u = transferedgecoarsen(tempelem,elemi{j},tree);
                tempPro_p = transferelem(tempelem,elemi{j},tree);
                % second coarsen
                [elemi{j-1},newHB2,tree] = uniformcoarsen(tempelem); % coarse bisection
                Pro_u{j-1} = transferedgecoarsen(elemi{j-1},tempelem,tree);
                Pro_u{j-1} = tempPro_u*Pro_u{j-1};
                Pro_p{j-1} = transferelem(elemi{j-1},tempelem,tree);
                Pro_p{j-1} = tempPro_p*Pro_p{j-1};
                newHB = [newHB1; newHB2];
            end
    end
    if (isempty(newHB)) || (size(elemi{j-1},1)< 2*N0)
    % no nodes are removed or it reaches the coarsest level
        NL = NL(j:end);
        break;
    end
    NL(j) = NL(j+1) - size(newHB,1); % update NL(k)
    nodei{j-1} = nodei{j};           % node in i-th level
    nodei{j-1}(newHB(:,1),:) = [];   % remove fine nodes
end
level = length(NL)-1;    % actual level
nodei = nodei(end-level+1: end);
elemi = elemi(end-level+1: end);
Pro_u = Pro_u(end-level+1: end);
Pro_p = Pro_p(end-level+1: end);

%% No coarsening
if level == 1  % no coarsening. use exact solver
    Np = size(p,1);
    Nu = size(f,1);
    bigA = [A B'; B sparse(Np,Np)];
    bigF = [f; g-mean(g)];
    bigu = zeros(Nu+Np,1);
    bigu(1:end-1) = bigA(1:end-1,1:end-1)\bigF(1:end-1);
    u = bigu(1:Nu);
    p = bigu(Nu+1:end);
    flag = 2; itStep = 0; err = norm(bigF-bigA*bigu)/norm(bigF); time = toc;
    info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));
    fprintf('direct solver time = %4.3g s,\n',time);
    Ai=[]; Bi=[]; invMt =[]; Pro_u =[]; Pro_p =[];
    return
end

%% Matrices in each level
Ai = cell(level,1);
Ai{level} = A;
Bi = cell(level,1);
Bi{level} = B;
invMt = cell(level,1);
area = simplexvolume(node,elem);
NTf = size(elem,1);
invMt{level} = spdiags(1./area,0,NTf,NTf);
NE = size(Pro_u{level-1},1);
ufreeDofj = ufreeDof(ufreeDof <= NE);
for j = level:-1:2
    % get mesh and data in the j-1 level
    elem = sortelem(elemi{j-1});
    node = nodei{j-1};
    [elem2edge,edge] = dofedge(elem);
    [Dlambda,area,elemSign] = gradbasis(node,elem);
    NTj = size(elem,1);

    % assemble matrix using the mesh information.
    % Mv: Lumped mass matrix for vertex: P1 element
    Mv = accumarray([elem(:,1);elem(:,2);elem(:,3)],[area;area;area]/3,[N,1]);
    Mv((Mv == 0)) = [];
    Nj = length(Mv);
    invMv = spdiags(1./Mv,0,Nj,Nj);
    Mv = spdiags(Mv,0,Nj,Nj);
    % Me: Mass matrix for RT0 element
    Me = getmassmatvec(elem2edge,area,Dlambda,'RT0');
    %invMt: the inverse of Mass matrix for P0 element
    invMt{j-1} = spdiags(1./area,0,NTj,NTj);
    % B: - divergence operator
    Bj = -icdmat(double(elem2edge),elemSign*[1 -1 1]);
    % C: curl operator
    C = icdmat(double(edge),[-1 1]);
    % R: weak rot operator
    R = invMv*C'*Me;
    % Vector Laplacian
    Ai{j-1} =  Bj'*invMt{j-1}*Bj + R'*Mv*R;

    % find free dof in each level
    isFixedDof = false(size(Ai{j-1},1),1);
    isDirichlet = false(size(Ai{j-1},1),1);
    % add coarsening into bdFlag
    bdFlag = setboundary(node,elem,'Dirichlet');
    if ~isempty(bdFlag)
        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        isFixedDof(isDirichlet) = true;
        ufreeDofc = find(~isFixedDof);
    end

    % modify transfer operators
    Pro_u{j-1} = Pro_u{j-1}(ufreeDofj,ufreeDofc);
    Ai{j-1} = Ai{j-1}(ufreeDofc,ufreeDofc);
    Bi{j-1} = Bj(:,ufreeDofc);
    ufreeDofj = ufreeDofc;
end
clear nodei elemi
end % end for setup

%% Only set up the transfer operators
if strcmp(solver,'NO')
    flag = 0; itStep = 0; err = 0; time = toc;
    info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));
    return
end

%% No need of set up
if setupflag == false
    Ai = varargin{1};
    Bi = varargin{2};
   invMt = varargin{3};
   Pro_u = varargin{4};
   Pro_p = varargin{5};
   level = size(Ai,1);
end

%% Construct other necessary operators
Nu = zeros(level,1);
Nu(level) = size(A,1);
Np = zeros(level,1);
Np(level) = size(B,1);
bigAi = cell(level,1);
bigAi{level} = [Ai{level} Bi{level}'; Bi{level} sparse(Np(level),Np(level))];
Res_u = cell(level,1);
Res_p = cell(level,1);
for j = level:-1:2
    Res_u{j} = Pro_u{j-1}';
    Res_p{j} = Pro_p{j-1}';
    Nu(j-1) = size(Ai{j-1},1);
    Np(j-1) = size(Bi{j-1},1);
    bigAi{j-1} = [Ai{j-1} Bi{j-1}'; Bi{j-1} sparse(Np(j-1),Np(j-1))];
end
Ndof = Nu + Np;

%% Smoothers
Su = cell(level,1);
Sp = cell(level,1);
for k = 1:level
    Sp{k} = tril(Bi{k}*(Bi{k})');
    Su{k} = tril(Ai{k});
end

%% Solver
bigF = [f; g-mean(g)];
bigu = [u(ufreeDof); p];
bigr = bigF - bigAi{level}*bigu;
nb = norm(bigF);
err = zeros(maxIt,1);
err(1) = norm(bigr)/nb;
k = 1;
% condest(bigAi{J}(1:end-1,1:end-1))
if strcmp(solver,'GMRES') % GMRES solver
    restart = 50; prefunc = @(r)kcycle(r,level);
    tic;
    [bige, itStep, relRes_u] = PFGMRES(bigAi{level}, bigr, bigu, maxIt, restart, tol, prefunc,0);
    bigu = bigu + bige;
    time = toc;
    fprintf('itStep=%d, relRes_u = %10.6g, time = %4.3g s,\n', ...
             itStep, relRes_u(itStep), time);
else  % MG
    while (max(err(k)) > tol) && (k <= maxIt)
        k = k + 1;
        % one multigrid cycle
        switch (solver)
            case 'VCYCLE'
                biguerr = vcycle(bigr);
            case 'WCYCLE'
                biguerr = wcycle(bigr);
        end
        % update solution
        bigu = bigu + biguerr;
        % compute residual
        bigr = bigr - bigAi{level}*biguerr;
        % compute the relative errror
        err(k) = norm(bigr)/nb;
        if printlevel >= 2
            fprintf('#dof: %8.0u, MG %8s iter: %2.0u, err = %8.4e\n',...
                 Ndof(level), solver, k-1, err(k));
        end
    end
end
err = err(1:k);
itStep = k-1;
u = bigu(1:Nu(level));
p = bigu(Nu(level)+1:end);

%% Output
if k > maxIt
    flag = 1;
else
    flag = 0;
end
time = toc;
if printlevel >= 1
    fprintf('#dof: %6.0u,  #nnz: %6.0u, level: %2.0u  MG %6s iter: %2.0u,  err = %8.4e,  time = %4.2g s\n',...
             Ndof(level), nnz(bigAi{level}), level, solver, itStep, err(end), time)
end
if (flag == 1) && (printlevel>0)
   fprintf('NOTE: the iterative method does not converge! \n');
end
info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions vcycle, wcycle
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Vcycle MG
    function e = vcycle(r,J)
        if nargin < 2, J = level; end
        if J == 1 % exact solver in the coaresest grid
           e = zeros(size(r));
           e(1:end-1) = bigAi{J}(1:end-1,1:end-1)\r(1:end-1);
           e(Nu(J)+1:end) = e(Nu(J)+1:end)-mean(e(Nu(J)+1:end));
           return
        end
        ru = r(1:Nu(J));
        rp = r(Nu(J)+1:end);

        % pre-smoothing in the fine grid
        [eu,ep] = DGSRT0(Ai{J},Bi{J},Bi{J}',zeros(Nu(J),1),zeros(Np(J),1),...
                         ru,rp,smoothingstep,Su{J},Sp{J},invMt{J});

        % form residual and restrict onto coarse grid
        ruc = Res_u{J}*(ru - Ai{J}*eu-(Bi{J})'*ep);
        rpc = Res_p{J}*(rp - Bi{J}*eu);

        % coarse grid correction twice
        rc = [ruc;rpc];
        ec = vcycle(rc,J-1);

        % prolongate coarse grid correction to the fine grid
        tempeu = Pro_u{J-1}*ec(1:Nu(J-1));
        tempep = Pro_p{J-1}*ec(Nu(J-1)+1:end);
        eu = eu + tempeu;
        ep = ep + tempep;

        % post-smoothing in the fine grid
        [eu,ep] = DGSRT0(Ai{J},Bi{J},Bi{J}',eu,ep,ru,rp,smoothingstep,...
                         Su{J},Sp{J},invMt{J});
        e = [eu;ep];
    end

%% Wcycle MG
    function e = wcycle(r,J)
        if nargin < 2, J = level; end
        if J == 1 % exact solver in the coaresest grid
           e = zeros(size(r));
           e(1:end-1) = bigAi{J}(1:end-1,1:end-1)\r(1:end-1);
           e(Nu(J)+1:end) = e(Nu(J)+1:end)-mean(e(Nu(J)+1:end));
           return
        end
        ru = r(1:Nu(J));
        rp = r(Nu(J)+1:end);

        % pre-smoothing in the fine grid
        [eu,ep] = DGSRT0(Ai{J},Bi{J},Bi{J}',zeros(Nu(J),1),zeros(Np(J),1),...
                         ru,rp,smoothingstep,Su{J},Sp{J},invMt{J});

        % form residual and restrict onto coarse grid
        ruc = Res_u{J}*(ru - Ai{J}*eu-(Bi{J})'*ep);
        rpc = Res_p{J}*(rp - Bi{J}*eu);

        % coarse grid correction twice
        rc = [ruc;rpc];
        ec = wcycle(rc,J-1);
        ec = ec + wcycle(rc-bigAi{J-1}*ec,J-1);

        % prolongate coarse grid correction to the fine grid
        tempeu = Pro_u{J-1}*ec(1:Nu(J-1));
        tempep = Pro_p{J-1}*ec(Nu(J-1)+1:end);
        eu = eu + tempeu;
        ep = ep + tempep;

        % post-smoothing in the fine grid
        [eu,ep] = DGSRT0(Ai{J},Bi{J},Bi{J}',eu,ep,ru,rp,smoothingstep,...
                         Su{J},Sp{J},invMt{J});
        e = [eu;ep];
    end

%% DGS for RT0
    function [u,p] = DGSRT0(A,B,Bt,u,p,f,g,itStep,Su,Sp,invMp)
        for s = 1: itStep
            % Step 1: relax Momentum eqns by G-S
            for i = 1:2
                u = u + Su\(f-Bt*p-A*u);
            end
            % Step 2: relax Continuity eqns by G-S
            rp = g - B*u;
            dq = Sp\rp;
            % Step 3: update u and p
            u = u + Bt*dq;
            p = p - invMp*rp;
%             p = p - Sp\(B*(A*(Bt*dq)));
        end
    end
end


function test_failed = test_libltfat_fold(varargin)

test_failed = 0;

fprintf(' ===============  %s ================ \n',upper(mfilename));

definput.flags.complexity={'double','single'};
[flags]=ltfatarghelper({},definput,varargin);
dataPtr = [flags.complexity, 'Ptr'];

Larr =     [10 11  10   10  101 101];
Lfoldarr = [3  16   3    3    1   2];
shiftarr = [0   0  -5 -101   -3  11];

for do_complex = 0:1
    complexstring = '';
    if do_complex, complexstring = 'complex'; end
    funname = makelibraryname('fold_array',flags.complexity,do_complex);

    for lId = 1:numel(Larr)
        L     = Larr(lId);
        Lfold = Lfoldarr(lId);
        shift = shiftarr(lId);

        if do_complex
            z = (1:max(L,Lfold)) + 1i*(max(L,Lfold):-1:1);
            z(L+1:end) = 0;
            zi = complex2interleaved(z);
            zout = complex2interleaved(randn(1,Lfold) + 1i*randn(1,Lfold));
        else
            z = (1:max(L,Lfold));
            z(L+1:end) = 0;
            zi = z;
            zout = randn(1,Lfold);
        end

        ziPtr = libpointer(dataPtr,zi);
        zoutPtr = libpointer(dataPtr,zout);

        periods = ceil(L/Lfold);
        fext = postpad(z,periods*Lfold);
        ffoldtrue = circshift(sum(reshape(fext, Lfold, periods),2).',[0,shift]);
        ffold2ndtrue = sum(reshape(circshift(fext,[0,shift]), Lfold, periods),2).';

        errTrues = norm(ffoldtrue - ffold2ndtrue);

        status = calllib('libltfat',funname,ziPtr,L,shift,Lfold,zoutPtr);

        if do_complex
            res = norm(ffoldtrue - interleaved2complex(zoutPtr.Value));
        else
            res = norm(ffoldtrue - zoutPtr.Value);
        end

        [test_failed,fail]=ltfatdiditfail(res+status,test_failed,0);
        fprintf(['FOLD OP L:%3i, Lfold:%3i, shift:%3i, %s %s %s %s\n'],L,Lfold,shift,flags.complexity,complexstring,ltfatstatusstring(status),fail);

        status = calllib('libltfat',funname,ziPtr,L,shift,Lfold,ziPtr);

        if do_complex
            res = norm(ffoldtrue - postpad(interleaved2complex(ziPtr.Value),Lfold));
        else
            res = norm(ffoldtrue - ziPtr.Value(1:Lfold));
        end

        [test_failed,fail]=ltfatdiditfail(res+status,test_failed,0);
        fprintf(['FOLD IP L:%3i, Lfold:%3i, shift:%3i, %s %s %s %s\n'],L,Lfold,shift,flags.complexity,complexstring,ltfatstatusstring(status),fail);
    end
end

%interleaved2complex(zoutPtr.Value)