classdef ballfunv
%BALLFUNV   BALLFUNV class for representing vector-valued functions on the unit ball.
%
%  Class for approximating smooth vector-valued functions defined on the unit ball.
%
% BALLFUNV(F, G, H) constructs a BALLFUNV object representing the vector-valued
% function [F;G;H] on the unit ball. F, G, and H may be BALLFUN objects, function
% handles or scalars. If they are function handles, then those function handles
% should be vectorized.
%
% See also BALLFUN.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CLASS PROPERTIES:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    properties

        comp   % The three components Vx, Vy, Vz of a BALLFUNV

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS CONSTRUCTOR:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = false )
        % Take the 3 components
        function F = ballfunv(varargin)

            % Return an empty BALLFUNV:
            if ( (nargin == 0) || isempty(varargin) )
                F.comp = {};
                return
            end

            % If the argument is a BALLFUNV, nothing to do:
            if ( isa(varargin{1}, 'ballfunv') )
                F = varargin{1};
                return
            end

            % BALLFUNV objects are vector-valued so complain if there
            % are less than 3 components:
            if ( numel(varargin) < 2 )
                error('BALLFUNV:ballfunv', ...
                    'Less than three components is not supported.')
            end

            % BALLFUNV objects cannot contain more than five components.
            % Complain if we have been given six or more.
            if ( numel(varargin) > 5 )
                error('BALLFUNV:ballfunv', ...
                    'More than four components is not supported.')
            end

            % Create a BALLFUNV from 3 ballfun or function handles
            if ( numel(varargin) == 3 )
                for jj = 1:3
                   if isa(varargin{jj}, 'ballfun') == 0
                       varargin{jj} = ballfun( varargin{jj} );
                   end
                end

                fh{1} = varargin{1};
                fh{2} = varargin{2};
                fh{3} = varargin{3};
            end

            % Create a BALLFUNV from 3 function handles in spherical
            % coordinates
            if ( numel(varargin) == 4 )
                if strcmp(varargin{4}, 'spherical') || strcmp(varargin{4}, 'polar')
                    fh{1} = ballfun(varargin{1},'spherical');
                    fh{2} = ballfun(varargin{2},'spherical');
                    fh{3} = ballfun(varargin{3},'spherical');
                else
                    error('BALLFUNV:ballfunv', ...
                        'Input arguments not supported')
                end
            end

            F.comp = fh;
        end
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% STATIC METHODS:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = true )
        varargout = PT2ballfunv(P,T);
    end

    methods ( Access = private, Static = true )

    end

end


function [lineHandles textHandles] = hline(y, lineType, label, textPosition, axesHandle)

% Draws horizontal lines (specified by a vector of y)
%
% Except the first argument 'y', everything else are optional.
% To preserve input argument order, enter [] for unused input arguments
%
% lineType:     same as the ones used in plot(...)
% label:        simple strings applies to all lines
%               cell strings applies to each line
% textPosition: [x, y] bewteen 0 and 1 relative to the 'lower' end
%
% Author:      Hoi Wong (wonghoi.ee@gmail.com)
% Date:        02/14/2008
%
% Acknowledgement:  It was based on hline() written by Brandon Kuczenski.

    if( ~isvector(y) )
        error('y must be a vector');
    else
        y=y(:);     % Make sure it's column vector
    end

    if( ~exist('label', 'var') )        label = [];         end
    if( ~exist('lineType', 'var') )     lineType = [];      end
    if( ~exist('axesHandle', 'var') )   axesHandle = gca;   end
    if( ~exist('textPosition', 'var') ) textPosition = [];  end

    if( isempty(axesHandle) )           axesHandle = gca;   end

    if( isempty(textPosition) )
        textPositionX = 0.02;
        textPositionY = 0.02;
    elseif( isscalar(textPosition) )
        textPositionX = textPosition;
        textPositionY = 0.02;
    elseif( length(textPosition)~=2 )
        error('Invalid textPosition');
    else
        textPositionX = textPosition(1);
        textPositionY = textPosition(2);
    end
    clear textPosition;     % Avoid typos for similarly named variables

    holdState = ishold(axesHandle);
    hold(axesHandle, 'on');

    xLimits=get(axesHandle,'xlim');             % Row vector
    Xlimits=repmat(xLimits', 1, length(y));

    % Example: for horizontal lines
    % X = [2 2        Y = [3 4
    %      5 5];           3 4];

    if( isempty(lineType) )
        lineHandles = plot(axesHandle, Xlimits, [y';y']);
    else
        lineHandles = plot(axesHandle, Xlimits, [y';y'], lineType);
    end

    if( ~isempty(label) )
        yLimits = get(axesHandle,'ylim');
        yLowerLimit = yLimits(2);
        yUpperLimit = yLimits(1);
        yRange      = yUpperLimit - yLowerLimit;
        yPosition   = y-textPositionY*yRange;

        xUpperLimit = xLimits(2);
        xLowerLimit = xLimits(1);
        xRange      = xUpperLimit - xLowerLimit;
        xPosition   = xLowerLimit+textPositionX*xRange;
        Xposition   = repmat(xPosition, length(y), 1);

        % textHandle is a vector correspond to the line
        textHandles = text(Xposition, yPosition, label, 'Parent', axesHandle);

        for k=1:length(y)
            % Set the text colors to be identical to line colors
            set( textHandles(k), 'color', get(lineHandles(k), 'color') );
        end
    end

    if( holdState==false )
        hold(axesHandle, 'off');
    end
    % this last part is so that it doesn't show up on legends
    set(lineHandles,'tag','hline','handlevisibility','off')


function msm_to_mm_coordinate_integer_symmetric ( output_filename, a )

%*****************************************************************************80
%
%% MSM_TO_MM_COORDINATE_INTEGER_SYMMETRIC writes a "matrix coordinate integer symmetric" Matrix Market file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string OUTPUT_FILENAME, the name of the file to which the information
%    should be written.
%
%    Input, sparse matrix A, the NROW by NCOL matrix, stored in MATLAB sparse
%    matrix format, which is to be written to the file.
%
  [ nrow, ncol ] = size ( a );
  nnzeros = nnz ( a );

  fid = fopen ( output_filename, 'wt+' );

  if ( fid < 0 );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MSM_TO_MM_COORDINATE_INTEGER_SYMMETRIC - Fatal error!\n' );
    fprintf ( 1, '  Cannot open the output file.\n' );
    error ( 'MSM_TO_MM_COORDINATE_INTEGER_SYMMETRIC - Fatal error!');
  end;

  fprintf ( fid, '%%%%MatrixMarket matrix coordinate integer symmetric\n');
  fprintf ( fid, '%%%%  Created by MSM_TO_MM_COORDINATE_INTEGER_SYMMETRIC.M\n' );
  fprintf ( fid, '  %d  %d  %d\n', nrow, ncol, nnzeros );

  for j = 1 : ncol
    [ rows, temp, vals ] = find ( a(:,j) );
    sz = size ( rows );
    for k2 = 1 : sz
      if ( j <= rows(k2) )
        fprintf ( fid, '  %d  %d  %d\n', rows(k2), j, vals(k2) );
      end
    end

  end

  fclose ( fid );

  return
end


%% FAST Algorithm for Corner Detection
%
% In this demo, we will:
%
% * understand the basics of FAST algorithm
% * find corners using OpenCV functionalities for FAST algorithm.
%
% Sources:
%
% * <https://docs.opencv.org/3.2.0/df/d0c/tutorial_py_fast.html>
%

%% Theory
%
% We saw several feature detectors and many of them are really good. But when
% looking from a real-time application point of view, they are not fast
% enough. One best example would be SLAM (Simultaneous Localization and
% Mapping) mobile robot which have limited computational resources.
%
% As a solution to this, FAST (Features from Accelerated Segment Test)
% algorithm was proposed by Edward Rosten and Tom Drummond in their paper
% "Machine learning for high-speed corner detection" in 2006 (later revised it
% in 2010). A basic summary of the algorithm is presented below. Refer to the
% original paper for more details (All the images are taken from original
% paper).
%
%% Feature Detection using FAST
%
% * Select a pixel $p$ in the image which is to be identified as an interest
%   point or not. Let its intensity be $I_p$.
% * Select appropriate threshold value $t$.
% * Consider a circle of 16 pixels around the pixel under test (See the image
%   below)
%
% <<https://docs.opencv.org/3.2.0/fast_speedtest.jpg>>
%
% * Now the pixel $p$ is a corner if there exists a set of $n$ contiguous
%   pixels in the circle (of 16 pixels) which are all brighter than $I_p + t$,
%   or all darker than $I_p - t$ (Shown as white dash lines in the above
%   image). $n$ was chosen to be 12.
% * A *high-speed test* was proposed to exclude a large number of non-corners.
%   This test examines only the four pixels at 1, 9, 5 and 13 (First 1 and 9
%   are tested if they are too brighter or darker. If so, then checks 5 and
%   13). If $p$ is a corner, then at least three of these must all be brighter
%   than $I_p + t$ or darker than $I_p - t$. If neither of these is the case,
%   then $p$ cannot be a corner. The full segment test criterion can then be
%   applied to the passed candidates by examining all pixels in the circle.
%
% This detector in itself exhibits high performance, but there are several
% weaknesses:
%
% * It does not reject as many candidates for |n < 12|.
% * The choice of pixels is not optimal because its efficiency depends on
%   ordering of the questions and distribution of corner appearances.
% * Results of high-speed tests are thrown away.
% * Multiple features are detected adjacent to one another.
%
% First 3 points are addressed with a machine learning approach. Last one is
% addressed using non-maximal suppression.
%
%% Machine Learning a Corner Detector
%
% * Select a set of images for training (preferably from the target
%   application domain)
% * Run FAST algorithm in every images to find feature points.
% * For every feature point, store the 16 pixels around it as a vector. Do it
%   for all the images to get feature vector $P$.
% * Each pixel (say $x$) in these 16 pixels can have one of the following
%   three states:
%
% <<https://docs.opencv.org/3.2.0/fast_eqns.jpg>>
%
% * Depending on these states, the feature vector $P$ is subdivided into 3
%   subsets, $P_d$, $P_s$, $P_b$.
% * Define a new boolean variable, $K_p$, which is true if $p$ is a corner and
%   false otherwise.
% * Use the ID3 algorithm (decision tree classifier) to query each subset
%   using the variable $K_p$ for the knowledge about the true class. It
%   selects the $x$ which yields the most information about whether the
%   candidate pixel is a corner, measured by the entropy of $K_p$.
% * This is recursively applied to all the subsets until its entropy is zero.
% * The decision tree so created is used for fast detection in other images.
%
%% Non-maximal Suppression
%
% Detecting multiple interest points in adjacent locations is another problem.
% It is solved by using Non-maximum Suppression.
%
% * Compute a score function, $V$ for all the detected feature points. $V$ is
%   the sum of absolute difference between $p$ and 16 surrounding pixels
%   values.
% * Consider two adjacent keypoints and compute their $V$ values.
% * Discard the one with lower $V$ value.
%
%% Summary
%
% It is several times faster than other existing corner detectors.
%
% But it is not robust to high levels of noise. It is dependent on a
% threshold.
%

%% FAST Feature Detector in OpenCV
%
% It is called as any other feature detector in OpenCV. If you want, you can
% specify the threshold, whether non-maximum suppression to be applied or not,
% the neighborhood to be used etc.
%
% For the neighborhood, three flags are defined, |TYPE_5_8|, |TYPE_7_12|, and
% |TYPE_9_16|. Below is a simple code on how to detect and draw the FAST
% feature points.
%

%%
% load source image
img = cv.imread(fullfile(mexopencv.root(),'test','blox.jpg'), 'Grayscale',true);

%%
% create FAST object with default values, and find keypoints
fast = cv.FastFeatureDetector();
kp1 = fast.detect(img);
fprintf('%d keypoints with NonmaxSuppression\n', numel(kp1));

%%
% disable |NonmaxSuppression| and find keypoints
fast.NonmaxSuppression = false;
kp2 = fast.detect(img);
fprintf('%d keypoints without NonmaxSuppression\n', numel(kp2));

%%
% draw keypoints from both cases, and show results
out1 = cv.drawKeypoints(img, kp1, 'Color',[255 0 0]);
out2 = cv.drawKeypoints(img, kp2, 'Color',[255 0 0]);
subplot(121), imshow(out1), title('FAST w/ NonmaxSuppression')
subplot(122), imshow(out2), title('FAST w/o NonmaxSuppression')

%% Additional Resources
%
% * Edward Rosten and Tom Drummond, "Machine learning for high speed corner
%   detection" in 9th European Conference on Computer Vision, vol. 1, 2006,
%   pp. 430-443.
% * Edward Rosten, Reid Porter, and Tom Drummond, "Faster and better: a
%   machine learning approach to corner detection" in IEEE Trans. Pattern
%   Analysis and Machine Intelligence, 2010, vol 32, pp. 105-119.
%


function blas0_test03 ( )

%*****************************************************************************80
%
%% BLAS0_TEST03 tests R8_SIGN.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 March 204
%
%  Author:
%
%    John Burkardt
%
  test_num = 5;
  x_test = [ -1.25, -0.25, 0.0, +0.5, +9.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BLAS0_TEST03\n' );
  fprintf ( 1, '  R8_SIGN returns the sign of a number.\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num
    x = x_test(test);
    fprintf ( 1, '  %8f  %8f\n', x, r8_sign ( x ) );
  end

  return
end


function [Y,FS] = spm_DEM_play_song(qU,T);
% displays the song-bird images specified by the states in qU
% FORMAT [Y,FS] = spm_DEM_play_song(qU,T);
%
% qU   - conditional moments of states (see spm_DEM)
% T    - number of seconds over which to play the sound
%
% Y    - sound image
% FS   - sampling rate (Hz)
%
% A button press on the spectrogram will play the song
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_DEM_play_song.m 6044 2014-06-14 10:22:46Z karl $

% load frequency modes
%--------------------------------------------------------------------------
try, T;           catch, T = 2;  end
try, v = qU.v{1}; catch, v = qU; end

% create sound image
%==========================================================================
[Nm m] = size(v);

% frequencies
%--------------------------------------------------------------------------
Hf  = 5000;                                % upper frequency (Hz)
Lf  = 2500;                                % lower frequency (Hz)
Nf  = 64;                                  % number of frequency bin
Hz  = linspace(Lf,Hf,64)';                 % frequencies
FS  = 2*Hz(end);                           % sampling rate (Hz)
k   = Hz/Hz(1);                            % cycles per window
n   = FS/Hz(1);                            % window length
N   = FS*T;                                % number of sonogram bins
R   = fix(N/m);                            % interpolation factor
N   = R*m;
pst = (1:N)/FS;                            % peristimulus time
sf  = 2*64^2;                              % dispersion of frequencies

% resample temporal modes
%--------------------------------------------------------------------------
for i = 1:Nm
    V(i,:) = spm_interp(v(i,:),R);
end

% create sonogram sound
%--------------------------------------------------------------------------
b     = V(1,:);                            % amplitude modulation
f     = V(2,:);                            % frequency modulation
b     = abs(b);
b     = b/max(b);
b     = tanh((b - 1/2)*6) + 1;
f     = 32*f + Lf;
S     = sparse(Nf,N);
for i = 1:N
    s      = b(i)*exp(-(Hz - f(i)).^2/sf);
    s      = sparse(s.*(s > exp(-4)));
    S(:,i) = s;
end

% inverse Fourier transform
%--------------------------------------------------------------------------
Y   = spm_iwft(S,k,n);
Y   = Y/max(Y);

% Graphics
%==========================================================================
colormap('pink')
imagesc(pst,Hz,abs(S))
axis xy
xlabel('time (sec)')
ylabel('Frequency (Hz)')

% set sound data
%--------------------------------------------------------------------------
h      = get(gca,'Children');
set(h(1),'Userdata',{Y,FS})
set(h(1),'ButtonDownFcn','spm_DEM_ButtonDownFcn')


%script test recursive
%%%%% quantitative differences: because of the way the level is updated
%%%%% (should be analytically equivalent but not numerically). Small
%%%%% deiations for 2-ToM (but accumulates) larger for 3-ToM
x=zeros(2,101); xtest=x;
y1=VBA_random ('Bernoulli', 0.7, 100, 1);
inF.player=1;
inF.lev=0;
Par=0; %theta
for i=1:100
    x(:,i+1)=RecToMfunction(x(:,i),Par,y1(i),inF);
    xtest(:,i+1)=evolution0bisND(xtest(:,i),Par,y1(i),inF);
end
%%
N=1000
xtest=zeros(8,N+1); x=zeros(9,N+1);
y1=VBA_random('Bernoulli', 0.7, N, 1);
y2=VBA_random('Bernoulli', 0.5, N, 1);
HS=cat(3,[1,0;0,1],[0,1;1,0]);
inF.player=1;
inF.lev=1;
inF.game=HS;
inF.fobs=@ObsRecGen;
inF.indParev=1;
inF.indParobs=1;
inF.dummyPar=[1;0];
in={HS,1};
Par=0; %theta
for i=1:N
    [x(:,i+1),indlev]=RecToMfunction(x(:,i),Par,[y2(i);y1(i)],inF);
    xtest(:,i+1)=evolution1bisND(xtest(:,i),Par,[y2(i);y1(i)],in);
end

%%%%compare
indcorr1=[1:2,6,8,7,9,3:4];
xcomp=x(indcorr1,:);
find(abs(xcomp-xtest)>1e-15)
%%
N=50;

y1=VBA_random('Bernoulli',0.7,100,1);
y2=VBA_random('Bernoulli',0.5,100,1);
%%
xtest=zeros(23,N+1); x=zeros(26,N+1);
HS=cat(3,[1,0;0,1],[0,1;1,0]);
inF.player=1;
inF.lev=2;
inF.game=HS;
inF.fobs=@ObsRecGen;
inF.indParev=1;
inF.indParobs=1;
inF.dummyPar=[1;0];
in={HS,1};
Par=0; %theta
for i=1:N
    [x(:,i+1),indlev]=RecToMfunction(x(:,i),Par,[y2(i);y1(i)],inF);
    xtest(:,i+1)=evolution2bisND(xtest(:,i),Par,[y2(i);y1(i)],in);
end
indcorr2=[2:5,10+indcorr1,20,21,7,9,8,10,23,25,24,26,1];
xcomp=x(indcorr2,:);
xcomp(end,:)=-xcomp(end,:);
%%%%compare
find(abs(xcomp-xtest)>1e-5)
%%
N=50;
xtest=zeros(53,N+1); x=zeros(60,N+1);
y1=VBA_random('Bernoulli',0.7,100,1);
y2=VBA_random('Bernoulli',0.5,100,1);
HS=cat(3,[1,0;0,1],[0,1;1,0]);
inF.player=1;
inF.lev=3;
inF.game=HS;
inF.fobs=@ObsRecGen;
inF.indParev=1;
inF.indParobs=1;
inF.dummyPar=[1;0];
in={HS,1};
Par=0; %theta
for i=1:3
    [x(:,i+1),indlev]=RecToMfunction(x(:,i),Par,[y2(i);y1(i)],inF);
    xtest(:,i+1)=evolution3bisND(xtest(:,i),Par,[y2(i);y1(i)],in);
end
 indcorr3=[3:6,11+indcorr1,21,22,27+indcorr2,54,55,8,10,9,11,24,26,25,27,57,59,58,60,1,2];
 xcomp=x(indcorr3,:);
 xcomp(37,:)=-xcomp(37,:); %estimation level 2

%%%%compare
find(abs(xcomp-xtest)>1e-4)

%%
N=10;
inF.lev=7;
%xtest=zeros(53,N+1);
x=zeros(sizeXrec(inF.lev,2),N+1);
y1=VBA_random('Bernoulli',0.7,100,1);
y2=VBA_random('Bernoulli',0.5,100,1);

HS=cat(3,[1,0;0,1],[0,1;1,0]);
inF.player=1;

inF.game=HS;
inF.fobs=@ObsRecGen;
inF.indParev=1;
inF.indParobs=1;
inF.dummyPar=[1;0];
in={HS,1};
Par=0; %theta
for i=1:N
    [x(:,i+1),indlev]=RecToMfunction(x(:,i),Par,[y2(i);y1(i)],inF);
   % xtest(:,i+1)=evolution3bisND(xtest(:,i),Par,[y2(i);y1(i)],in);
end
%  indcorr3=[3:6,11+indcorr1,21,22,27+indcorr2,54,55,8,10,9,11,24,26,25,27,57,59,58,60,1,2];
%  xcomp=x(indcorr3,:);
%  xcomp(37,:)=-xcomp(37,:); %estimation level 2
%
% %%%%compare
% find(abs(xcomp-xtest)>1e-10)
%%
N=100;
 x=zeros(264,N+1);
y1=VBA_random('Bernoulli',0.7,100,1);
y2=VBA_random('Bernoulli',0.5,100,1);
HS=cat(3,[1,0;0,1],[0,1;1,0]);
inF.player=1;
inF.lev=5;
inF.game=HS;
inF.fobs=@ObsRecGen;
inF.indParev=1;
inF.indParobs=1;
inF.dummyPar=[1;0];
in={HS,1};
Par=0; %theta
for i=1:N
    [x(:,i+1),indlev]=RecToMfunction(x(:,i),Par,[y2(i);y1(i)],inF);
end


function result=BF0NLS(speechSignal,samplingFreq,plot_flag,prew_flag,Par_set)
% -----------------------------------------------------------------------
% format of the input and output arguments
%
% input:
% input arguments --> as the names suggested
% plot_flag: 0 when you do not want a plot  (0 in default)
% prew_flag: 0 when you do not want to prewhiten the signal (0 in default)
% Par_set:   the parameter set you can set, including the segment time,
% segment shift and F0 boundaries.
%
% Par_set.segmentTime is the segmentTime in seconds  (25 ms in default)
% Par_set.segmentShift is the segmentShift in seconds (10 ms in default)
% Par_set.f0Bounds is the F0 boundaries in Hz ([70 400] Hz in default)
%
% output:
% result.tt         -->   time vector
% result.ff         -->   Fundamental frequency estimates
%                         (when all the frames are considered as voiced)
% result.oo         -->   order estimates
%                         (when all the frames are considered as voiced)
% result.vv         -->   voicing probability
% result.best_ff    -->   Best fundamental frequency estimates (setting the F0=nan
%                         when voicing probability is less than .5)
% result.best_order -->   Best harmonic order estimates (setting the order=nan
%                         when voicing probability is less than .5)
%
% Example for for customize Par_set
%
% Par_set.segmentTime = 0.025; 25 ms for each segment (default value)
% Par_set.segmentShift = 0.01; 10 ms for segment shift (default value)
% Par_set.f0Bounds =[70, 400]; pitch is bounded between 70 to 400 Hz (default value)
%
%
% Written by Liming Shi, Aalborg 9000, Denmark
% Email: ls@create.aau.dk
% -----------------------------------------------------------------------

if nargin<3
%   do not use prewhitening in default, and do not plot
    plot_flag=0;
    prew_flag=0;
    Par_set=[];
end
if nargin<4
%   do not use prewhitening in default
    prew_flag=0;
    Par_set=[];
end
if nargin<5
%   use default parameter set
    Par_set=[];
end

%% resample to 16 KHz for tuned parameters. However, the sampling frequency can be changed.
fs_fine_tuned=16000;
speechSignal=resample(speechSignal,fs_fine_tuned,samplingFreq);
samplingFreq=fs_fine_tuned;
if isempty(Par_set)
    segmentTime = 0.025; %     seconds
    segmentShift = 0.010; % seconds
    f0Bounds = [70, 400]; % cycles/sample
    std_pitch=2;
else
    if isfield(Par_set,'segmentTime')
        segmentTime = Par_set.segmentTime; %     seconds
    else
        segmentTime = 0.025; %     seconds
    end
    if isfield(Par_set,'segmentShift')
        segmentShift = Par_set.segmentShift;
        % std_pitch --> standard deviation for the pitch for 10 ms frame shift.
        % This value shouold be modified if frame shift is shorter or longer, with a lower bound of .5 Hz.
        std_pitch=max(2/10*segmentShift*1000,0.5);
    else
        segmentShift = 0.010; % seconds
        % std_pitch --> standard deviation for the pitch for 10 ms frame shift.
        std_pitch=2;
    end
    if isfield(Par_set,'f0Bounds')
        f0Bounds = Par_set.f0Bounds; % cycles/sample
    else
        f0Bounds = [70, 400]; % cycles/sample
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% normalization step
sumE = sqrt(speechSignal'*speechSignal/length(speechSignal));
scale = sqrt(3.1623e-5)/sumE; % scale to -45-dB loudness level
speechSignal=speechSignal*scale;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nData = length(speechSignal);
% set up
f0Bounds=f0Bounds/samplingFreq;
segmentLength = round(segmentTime*samplingFreq/2)*2; % samples
nShift = round(segmentShift*samplingFreq); % samples
nSegments = floor((nData+segmentLength/2-segmentLength)/nShift)+1;

if prew_flag==0
    maxNoHarmonics = 10;
else
    if prew_flag==1
        maxNoHarmonics = 30;
    end
end
f0Estimator = BayesianfastF0NLS(segmentLength, maxNoHarmonics, f0Bounds, std_pitch/samplingFreq,.7);
speechSignal_padded=[zeros(segmentLength/2,1);speechSignal];
% do the analysis
idx = 1:segmentLength;
f0Estimates = nan(nSegments,1); % cycles/sample
order=nan(nSegments,1);
voicing_prob=nan(nSegments,1);
if prew_flag==0
    for ii = 1:nSegments
        speechSegment = speechSignal_padded(idx);
        [f0Estimates(ii),order(ii),voicing_prob(ii)]=f0Estimator.estimate(speechSegment,0);
        idx = idx + nShift;
    end
else
    if prew_flag==1
        for ii = 1:nSegments
            speechSegment = speechSignal_padded(idx);
            [f0Estimates(ii),order(ii),voicing_prob(ii)]=f0Estimator.estimate(speechSegment,1,segmentShift);
            idx = idx + nShift;
        end
    end
end

f0Estimates_remove_unvoiced=f0Estimates;
unvoiced_indicator=voicing_prob<.5;;
f0Estimates_remove_unvoiced(unvoiced_indicator)=nan;
order_remove_unvoiced=order;
order_remove_unvoiced(unvoiced_indicator)=nan;
timeVector = [(0:nSegments-1)*segmentShift]';

result.tt=timeVector;
result.ff=f0Estimates*samplingFreq;
result.oo=order;
result.vv=voicing_prob;
result.best_ff=f0Estimates_remove_unvoiced*samplingFreq;
result.best_order=order_remove_unvoiced;
if plot_flag==1
    figure;
    subplot(4,1,4)
    plot([0:length(speechSignal_padded)-1]/samplingFreq,speechSignal_padded/max(abs(speechSignal_padded)));
    xlim([0,(length(speechSignal_padded)-1)/samplingFreq])
    xlabel('Time [s]');
    ylabel('Amplitude');
    subplot(4,1,3)
%   plot the spectrogram
    window = gausswin(segmentLength);
    nOverlap = segmentLength-nShift;
    nDft = 2048;
    [stft, stftFreqVector] = ...
        spectrogram(speechSignal_padded, window, nOverlap, nDft, samplingFreq);
    powerSpectrum = abs(stft).^2;
    imagesc(timeVector, stftFreqVector, ...
    10*log10(dynamicRangeLimiting(powerSpectrum, 60)));
    axis xy;
    ylim([0,f0Bounds(2)*samplingFreq+100])
    hold on;
    plot(timeVector, result.best_ff, 'r-', 'linewidth',2);
    ylabel('Frequency [Hz]');
    subplot(4,1,2)
    plot(timeVector,result.best_order,'r.', 'linewidth',2)
    xlim([timeVector(1),timeVector(end)])
    ylabel('Order estimate')
    subplot(4,1,1)
    plot(timeVector,result.vv,'r-', 'linewidth',2)
    xlim([timeVector(1),timeVector(end)])
    ylabel('Voicing probability')
end

end


% SVMTRAIN - Trains a support vector machine incrementally
%            using the L1 soft margin approach developed by
%            Cauwenberghs for two-class problems.
%
% Syntax: [a,b,g,ind,uind,X_mer,y_mer,Rs,Q] = svmtrain(X,y,C,type,scale)
%         [a,b,g,ind,uind,X_mer,y_mer,Rs,Q] = svmtrain(X,y,C,type,scale,uind)
%         (trains a new SVM on the given examples)
%
%         [a,b,g,ind,uind,X_mer,y_mer,Rs,Q] = svmtrain(X,y,C)
%         [a,b,g,ind,uind,X_mer,y_mar,Rs,Q] = svmtrain(X,y,C,uind)
%         (trains the current SVM in memory on the given examples)
%
%      a: alpha coefficients
%      b: bias
%      g: partial derivatives of cost function w.r.t. alpha coefficients
%    ind: cell array containing indices of margin, error and reserve vectors
%         ind{1}: indices of margin vectors
%         ind{2}: indices of error vectors
%         ind{3}: indices of reserve vectors
%   uind: column vector of user-defined example indices (used for unlearning specified examples)
%  X_mer: matrix of margin, error and reserve vectors stored columnwise
%  y_mer: column vector of class labels (-1/+1) for margin, error and reserve vectors
%     Rs: inverse of extended kernel matrix for margin vectors
%      Q: extended kernel matrix for all vectors
%      X: matrix of training vectors stored columnwise
%      y: column vector of class labels (-1/+1) for training vectors
%      C: soft-margin regularization parameter(s)
%         dimensionality of C       assumption
%         1-dimensional vector      universal regularization parameter
%         2-dimensional vector      class-conditional regularization parameters (-1/+1)
%         n-dimensional vector      regularization parameter per example
%         (where n = # of examples)
%   type: kernel type
%           1: linear kernel        K(x,y) = x'*y
%         2-4: polynomial kernel    K(x,y) = (scale*x'*y + 1)^type
%           5: Gaussian kernel with variance 1/(2*scale)
%  scale: kernel scale
%
% Version 3.22e -- Comments to diehl@alumni.cmu.edu
%

function [a,b,g,ind,uind,X,y,Rs,Q] = svmtrain(X_new,y_new,C_new,varargin)

% flags for example state
MARGIN    = 1;
ERROR     = 2;
RESERVE   = 3;
UNLEARNED = 4;

% create a vector containing the regularization parameter
% for each example if necessary
if (length(C_new) == 1)              % same regularization parameter for all examples
   C_new = C_new*ones(size(y_new));
elseif (length(C_new) == 2)          % class-conditional regularization parameters
   flags = (y_new == -1);
   C_new = C_new(1)*flags + C_new(2)*(~flags);
end;

if (nargin >= 5)

   % define arguments
   type_new = varargin{1};
   scale_new = varargin{2};
   if (nargin == 6)
      uind_new = varargin(3);
   else
      uind_new = zeros(size(y_new));
   end;

   new_model = 1;
else

   % define arguments
   if (nargin == 4)
      uind_new = varargin(1);
   else
      uind_new = zeros(size(y_new));
   end;

   new_model = 0;
end;

% define global variables
global a;                     % alpha coefficients
global b;                     % bias
global C;                     % regularization parameters
global deps;                  % jitter factor in kernel matrix
global g;                     % partial derivatives of cost function w.r.t. alpha coefficients
global ind;                   % cell array containing indices of margin, error, reserve and unlearned vectors
global kernel_evals;          % kernel evaluations
global max_reserve_vectors;   % maximum number of reserve vectors stored
global perturbations;         % number of perturbations
global Q;                     % extended kernel matrix for all vectors
global Rs;                    % inverse of extended kernel matrix for margin vectors
global scale;                 % kernel scale
global type;                  % kernel type
global uind;                  % user-defined example indices
global X;                     % matrix of margin, error, reserve and unlearned vectors stored columnwise
global y;                     % column vector of class labels (-1/+1) for margin, error, reserve and unlearned vectors

% initialize variables
deps = 1e-3;
max_reserve_vectors = 3000;

if (new_model)

   num_examples = size(X_new,2);

   a = zeros(num_examples,1);
   b = 0;
   C = C_new;
   g = zeros(num_examples,1);
   ind = cell(4,1);
   ind{UNLEARNED} = 1:num_examples;
   kernel_evals = 0;
   perturbations = 0;
   Q = y_new';
   Rs = Inf;
   scale = scale_new;
   type = type_new;
   uind = uind_new;
   X = X_new;
   y = y_new;

else

   num_examples = size(X,2);
   num_new_examples = size(X_new,2);

   a = [a ; zeros(num_new_examples,1)];
   C = [C ; C_new];
   g = [g ; zeros(num_new_examples,1)];
   ind{UNLEARNED} = (1:num_new_examples) + num_examples;

   % assumes currently that there are no duplicate examples in the data - may not necessarily be true!
   Q_new = [y_new' ; (y(ind{MARGIN})*y_new').*kernel(X(:,ind{MARGIN}),X_new,type,scale)];

   Q = [Q Q_new];
   uind = [uind ; uind_new];
   X = [X X_new];
   y = [y ; y_new];

   num_examples = num_examples + num_new_examples;

end;

% begin incremental learning - enforce all constraints on each iteration
num_learned = 1;
disp('Beginning training.');
while (any(ind{UNLEARNED}))

   % randomly select example
   i = round(rand*(length(ind{UNLEARNED})-1)) + 1;
   indc = ind{UNLEARNED}(i);
%  indc = ind{UNLEARNED}(1);

	% learn example
   learn(indc,1);

   if (mod(num_learned,50) == 0)
      s = sprintf('Learned %d examples.',num_learned);
      disp(s);
   end;
   num_learned = num_learned + 1;

end;
if (mod(num_learned-1,50) ~= 0)
   s = sprintf('Learned %d examples.',num_learned-1);
   disp(s);
end;
disp('Training complete!');

% begin incremental learning - perform multiple passes through the data
% until all of the examples are learned
%while (any(ind{UNLEARNED}))
%   while (any(ind{UNLEARNED}))
%
%      % select example
%      indc = ind{UNLEARNED}(1);
%
%      % learn example
%      s = sprintf('\nLearning example %d...',indc);
%      disp(s);
%      learn(indc,0);
%
%   end;
%
%   % check to see if any reserve vectors are incorrectly classified
%   % if so, change their status to unlearned
%   ind_temp = find(g(ind{RESERVE}) < 0);
%   [ind{RESERVE},ind{UNLEARNED}] = move_ind(ind{RESERVE},ind{UNLEARNED},ind{RESERVE}(ind_temp));
%
%end;

% remove all but the closest reserve vectors from the dataset if necessary
if (length(ind{RESERVE}) == max_reserve_vectors)
   ind_keep = [ind{MARGIN} ind{ERROR} ind{RESERVE}];
   a = a(ind_keep);
   g = g(ind_keep);
   Q = Q(:,ind_keep);
   uind = uind(ind_keep);
   X = X(:,ind_keep);
   y = y(ind_keep);
   ind{MARGIN} = 1:length(ind{MARGIN});
   ind{ERROR} = length(ind{MARGIN}) + (1:length(ind{ERROR}));
   ind{RESERVE} = length(ind{MARGIN}) + length(ind{ERROR}) + (1:length(ind{RESERVE}));
end;

% summary statistics
s = sprintf('\nMargin vectors:\t\t%d',length(ind{MARGIN}));
disp(s);
s = sprintf('Error vectors:\t\t%d',length(ind{ERROR}));
disp(s);
s = sprintf('Reserve vectors:\t%d',length(ind{RESERVE}));
disp(s);
s = sprintf('Kernel evaluations:\t%d\n',kevals);
disp(s);


 function st = newfft_table_init(st, varargin)
%function st = newfft_table_init(st, varargin)
%|
%| Initialize structure for d-dimension NUFFT using table-based interpolator,
%| This should be called only by newfft for its 'table0' or 'table1' mode!
%| Note: default oversample factor is 2^11 or 2^13
%|
%| in
%|	st		structure initialized in newfft.m
%|			uses st.phasing to choose complex or real kernel
%|
%| option
%|	'how'	char	'slow' | 'ratio' | 'fast'
%|			(default depends on kernel type)
%|
%| out
%|	st	strum	(see newfft)
%|
%| Copyright 2008-7-11, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end

arg.how = '';
arg = vararg_pair(arg, varargin);

if isempty(arg.how)
	if streq(st.ktype, 'kb:', 3) % todo: others?
		arg.how = 'ratio';
	else
		arg.how = 'fast';
	end
end

st.phase_shift = []; % compute on-the-fly

Nd = st.Nd;
Jd = st.Jd;
Kd = st.Kd;
dd = length(Nd);
Ld = st.oversample;
if isempty(Ld)
	switch st.mode
	case 'table0'
		st.order = 0; % 0th order
		Ld = 2^11; % default
	case 'table1'
		st.order = 1; % 1st order
		Ld = 2^9; % default
	otherwise
		fail 'bad mode'
	end
end

% dimensionality of input space (usually 2 or 3)
if dd > 1 && length(Ld) == 1
	Ld = repmat(Ld, [1 dd]); % allow scalar L to be used for all dimensions
end
st.oversample = Ld;
st.Ld = Ld;

if dd ~= length(Jd) || dd ~= length(Kd) || dd ~= length(Ld)
	whos
	fail 'inconsistent dim'
end

% desired scaling factors, using fake omega=[0]
ktype_args = {'ktype', st.ktype}; % user's interpolator
if ~isempty(st.kb_m)
	ktype_args = {ktype_args{:}, 'kb_m', st.kb_m, 'kb_alf', st.kb_alf};
elseif ~isempty(st.alpha)
	ktype_args = {ktype_args{:}, 'alpha', st.alpha, 'beta', st.beta};
end

tmp = newfft(zeros(size(Nd)), st.Nd, 'Jd', st.Jd, 'Kd', st.Kd, ...
	'n_shift', 0*st.n_shift, ktype_args{:}, 'mode', 'sparse', ...
	'phasing', st.phasing);
st.sn = tmp.sn;

if streq(st.phasing, 'flipreal')
	iseven = @(n) mod(n,2) == 0;
	st.flips = iseven(st.Nd); % [1 dd]
end

% 1D tables for each dimension
for id = 1:dd

	ktype_args = {'ktype', st.ktype}; ... % user's interpolator
	if ~isempty(st.kb_m)
		ktype_args = {ktype_args{:}, ...
			'kb_m', st.kb_m(id), 'kb_alf', st.kb_alf(id)};
	elseif ~isempty(st.alpha)
		ktype_args = {ktype_args{:}, ...
			'alpha', {st.alpha{id}}, 'beta', {st.beta{id}}};
	end

	make_args = {Nd(id), Jd(id), Kd(id), Ld(id), ktype_args, st.phasing};
	st.h{id} = newfft_table_make(arg.how, make_args{:});

	if 0 % test fast vs slow
		[h0 t0] = newfft_table_make('slow', make_args{:});
		h = st.h{id};
		jf plc 2 2
		jf sub 1, plot(t0, real(h0), 'c-', t0, real(h), 'y.')
		jf sub 2, plot(t0, real(h0-h), 'y.')
		jf sub 3, plot(t0, imag(h0), 'c-', t0, imag(h), 'y.')
		jf sub 4, plot(t0, imag(h0-h), 'y.')
		max_percent_diff(h0, h)
	prompt
	end

	switch st.phasing
	case 'complex'
		if isreal(st.h{id})
			warn 'real kernel?'
%			st.h{id} = complex(st.h{id});
		end

	case {'real', 'flipreal', 'none'}
		if ~isreal(st.h{id})
			fail 'nonreal kernel bug'
		end

	otherwise
		fail bug
	end

end

st = strum(st, { ...
	'fft', @newfft_approx_for, '(x, [om])';
	'adj', @newfft_approx_adj, '(X, [om])';
	'interp_for', @newfft_table_for, '(Xk, [om])';
	'interp_adj', @newfft_table_adj, '(Xk, [om])';
	'plot', @newfft_table_plot, '()';
	});

% newfft_table_make()
function [h, t0] = newfft_table_make(how, N, J, K, L, ktype_args, phasing)

if streq(phasing, 'flipreal')
	phasing = 'none'; % trick: get pure real kernel (with no sign flips)
end
% note: if phasing is already 'real' then keep it that way!

mode_args = {'mode', 'sparse', 'Jd', J, 'n_shift', 0, 'phasing', phasing};

t0 = [-J*L/2:J*L/2]' / L; % [J*L+1]

switch how

% This is a slow and inefficient (but simple) way to get the table
% because it builds a huge sparse matrix but only uses 1 column!
case 'slow'

	om0 = t0 * 2*pi/K; % gam
	s1 = newfft(om0, N, 'Kd', K, ktype_args{:}, mode_args{:});
	h = full(s1.p(:,1));

% This way is "J times faster" than the slow way, but still not ideal.
% It works for any user-specified interpolator.
case 'fast'

	t1 = J/2-1 + [0:(L-1)]' / L;	% [L]
	om1 = t1 * 2*pi/K;		% * gam
	s1 = newfft(om1, N, 'Kd', K, ktype_args{:}, mode_args{:});
	h = col(full(s1.p(:,J:-1:1))); % [J*L+0]
%	h = [h; 0]; % [J*L+1]
	h = [h; h(1)]; % [J*L+1] - assuming symmetric

% This efficient way uses only "J" columns of sparse matrix!
% The trick to this is to use fake small values for N and K,
% which works for interpolators that depend only on the ratio K/N.
case 'ratio' % e.g., 'minmax:kb' | 'kb:*'

	Nfake = J;
	Kfake = Nfake * K/N;
	t1 = J/2-1 + [0:(L-1)]' / L;	% [L]
	om1 = t1 * 2*pi/Kfake;		% "gam"
	s1 = newfft(om1, Nfake, 'Kd', Kfake, ktype_args{:}, mode_args{:});
	h = col(full(s1.p(:,J:-1:1))); % [J*L+0]
%	h = [h; 0]; % [J*L+1]
	h = [h; h(1)]; % [J*L+1] assuming symmetric

	if streq(phasing, 'complex')
		h = h .* exp(1i * pi * t0 * (1/K - 1/Kfake)); % fix phase
	end

otherwise
	fail('bad type %s', how)
end

% newfft_table_plot()
function newfft_table_plot(st)
color = 'cym';
arg = {};
for id = 1:st.dd
	J = st.Jd(id);
	L = st.Ld(id);
	t0 = [-J*L/2:J*L/2]' / L; % [J*L+1]
	arg = {arg{:}, t0, real(st.h{id}), [color(id) '-']};
	arg = {arg{:}, t0, imag(st.h{id}), [color(id) '--']};
end
plot(arg{:})

% newfft_table_for()
function X = newfft_table_for(st, Xk, om)
if ~isvar('om') || isempty(om)
	om = st.om;
end
if isempty(om)
	fail 'need om or st.om'
end
if st.dd ~= size(om,2), fail('omega needs %d columns', dd), end

X = nufft_table_interp(st, Xk, st.order, st.flips, om);

% newfft_exact_adj()
function Xk = newfft_table_adj(st, X, om)
if ~isvar('om') || isempty(om)
	om = st.om;
end
if isempty(om)
	fail 'need om or st.om'
end
if st.dd ~= size(om,2), fail('omega needs %d columns', dd), end

Xk = nufft_table_adj(st, X, st.order, st.flips, om);


filename='Bridge_quad_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'PROJECTED GRADIENT'; incrementFactor = 1;
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.2;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function [status, results] = mrtrix_dwi2tensor (in_file, out_file, b_file, verbose)

%
% Calculate diffusion tensors.
%
% Parameters
% ----------
% in_file: The name of a diffusion file in .mif format
% out_file: The name of the resulting dti file in .mif format
% b_file: The name of a mrtrix format gradient vector file (default: 'encoding.b')
%
% Returns
% -------
% status: whether (0) or not (1) the operation succeeded
% results: the results of the operation in the terminal
%
% Notes
% -----
% http://www.brain.org.au/software/mrtrix/tractography/preprocess.html

if notDefined('verbose')
   verbose = true;
end

if notDefined('bkgrnd')
   bkgrnd = false;
end

if notDefined('b_file')
    b_file = 'encoding.b';
end

% This command generates  tensors:
cmd_str = sprintf('dwi2tensor %s %s -grad %s',in_file, out_file, b_file);

% Send it to mrtrix:
[status,results] = mrtrix_cmd(cmd_str, bkgrnd, verbose);


function [P,G] = fs_signrank(x_fname,y_fname,varargin)
%
%  Author: Martin Reuter
%
%  Extends Wilcoxon signed rank test for zero median to FS surfaces
%  If y_fname is passed, do a paired test, see fs_ranksum for independent
%  samples.
%
%  Can work in parallel mode (run 'matlabpool OPEN' first)
%
%  x_fname    file name of mgh file (stacked surfaces)
%  y_fname    optional: file name for second file to do a paired test
%
%  fs_signrank(...,'mask',MASK) selects a mask (e.g. cortex.label)
%
%  fs_signrank(...,'outsig',OUTSIG) specifies file to output p-values
%      output will be in FreeSurfer mgh format, signed - log10(p)
%
%  fs_signrank(...,'outgamma',OUTGAMMA specifies file to output gamma
%      output will be in FreeSurfer mgh format (see G below)
%
%  See signrank for more arguments ('method' and 'tail')
%
%  Return (only inside mask):
%
%  G          gamma: median(X)            for single sample
%                    median(X)-median(Y)  for paired samples
%
%  P          p-values of signed rank test
%

% parse and remove mask and outfile params:
oknames = {'mask' 'outsig' 'outgamma' 'tail' 'alpha' 'method'};
dflts   = {'' '' '' 'both' 0.05   '' };
[mask_fname , outsig, outgamma, tail] = internal.stats.parseArgs(oknames,dflts,varargin{:});
% remove mask and outfile:
del = zeros(size(varargin));
for i = 1:length(varargin)
  if (strcmp(varargin{i},'mask'))
    del(i) = 1;
    del(i+1) = 1;
  end
  if strcmp(varargin{i},'outsig')
    del(i) = 1;
    del(i+1) = 1;
  end
  if strcmp(varargin{i},'outgamma')
    del(i) = 1;
    del(i+1) = 1;
  end
end
varargin = varargin(find(del == 0));
%tail = getParamVal(tail,{'both' 'right' 'left'},'''tail''');
if (strcmp(tail,'both'))
  tail = 0;
elseif (strcmp(tail,'left'))
  tail = -1;
elseif (strcmp(tail,'right'))
  tail = 1;
else
  error('tail can only be "both","left" or "right"');
end

% read X from file:
if ( ischar(x_fname) && exist(x_fname,'file') )
  [X,Mx] = fs_read_Y(x_fname);
  nsubj = size(X,1);
  nvert = size(X,2);
else
  error(['Can not load ' x_fname ' as an mgh or mgz file']);
end

% if passed read Y from file:
Y = [];
if (  ~ isempty(y_fname) )
  if ischar(y_fname) && exist(y_fname,'file')
    [Y,My] = fs_read_Y(y_fname);
    if (size(X) ~= size(Y))
      error('Dimensions of X and Y do not agree');
    end
  else
      error(['Can not load ' y_fname ' as an mgh or mgz file']);
  end
end

% read mask from file:
mask = [1:nvert];
outside = [];
if ( ~ isempty(mask_fname) )
  mask = fs_read_label(mask_fname);
  zz = zeros(1,nvert);
  zz(mask) = ones(1,length(mask));
  outside = find(zz==0);
end

% process
n = length(mask);
Pt = ones(1,n);
if (isempty(Y))
  parfor i = 1:n
    Pt(i) = signrank(X(:,mask(i)),'',varargin{:});
  end
  G = median(X);
else
  parfor i = 1:n
    idx = mask(i);
    Pt(i) = signrank(X(:,idx),Y(:,idx),varargin{:});
  end
  G = median(X)-median(Y);
end
P = ones(1,nvert);
P (mask) = Pt;
G(outside) = zeros(1,length(outside));

% write (on full surface)
if ( ~ isempty(outsig) )
  Pl10 = -log10(P) .* sign(G);
  Mx.volsz(4) = 1;
  fs_write_Y(Pl10,Mx,outsig);
end

if ( ~ isempty(outgamma) )
  Mx.volsz(4) = 1;
  fs_write_Y(G,Mx,outgamma);
end

% return values (only inside mask):
P = Pt;
G = G(mask);
meanG = mean(G);


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal & Francois Alouges (c) 2017-2018           |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtHmxFemBemEFIEhalf.m                        |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & Francois Alouges            |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Solve fem/bem with PEC scatering problem      |
%|  `---'  |                Volumic ans surfacic EFIE formulation         |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

%%% PREPARATION
disp('~~~~~~~~~~~~~ PREPARATION ~~~~~~~~~~~~~')

% Spherical mesh
mesh = msh('sphere_1e3.msh');

% Normalization interior sphere
mesh.vtx = mesh.vtx/0.8;

% Interior boundary
bound = mesh.bnd;
Iint  = (sum(bound.ctr.*bound.nrm,2) <= 0);
int   = swap(bound.sub(Iint));

% Final volumn
ctr = mesh.ctr;
vol = mesh.sub(ctr(:,3)>=0);

% Boundaries
tmp  = vol.bnd;
tmp1 = setdiff(int,tmp);
dvol = setdiff(tmp,int);
ext  = union(tmp1,dvol);
int  = intersect(int,tmp);

figure
plot(ext)
hold on
plotNrm(ext,'w')
plot(dvol,'b')
plot(int,'r')
axis equal
alpha(0.5)

% Domaine volumique
omega = dom(vol,4);

% Domaines surfacic
sigma = dom(dvol,3);
gamma = dom(ext,3);

% Frequency adjusted to maximum edge size
stp = mesh.stp;
k   = 1/stp(2);
c   = 299792458;
f   = (k*c)/(2*pi);
disp(['Frequency : ',num2str(f/1e6),' MHz']);

% Accuracy
tol = 1e-3;
disp(['Accuracy : ',num2str(tol)]);

% Incident direction and field
X0 = [0 0 -1];
E  = [0 1  0]; % Polarization (+x for Theta-Theta and +y for Phi-Phi)
H  = cross(X0,E);

% Incident Plane wave (electromagnetic field)
PWE{1} = @(X) exp(1i*k*X*X0') * E(1);
PWE{2} = @(X) exp(1i*k*X*X0') * E(2);
PWE{3} = @(X) exp(1i*k*X*X0') * E(3);

% Incident wave representation
figure
plot(ext,real(PWE{2}(ext.vtx)))
title('Incident wave')
xlabel('X');   ylabel('Y');   zlabel('Z');
axis equal
view(0,10)

%%% FORMUMATIONS
disp('~~~~~~~~~~~~~ FORMULATIONS ~~~~~~~~~~~~~')

% Green kernel
Gxy    = @(X,Y) femGreenKernel(X,Y,'[exp(ikr)/r]',k);
Hxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]1',k) ;
Hxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]2',k) ;
Hxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]3',k) ;

% Surfacic finite elements for magnetic current
Jh = fem(ext,'RWG');
N1 = size(Jh.unk,1);

% Volumique finite elements for electric field
Eh = fem(vol,'NED');
Eh = dirichlet(Eh,int);
N2 = size(Eh.unk,1);

% Restriction matrix for regularization
[~,I1,I2] = intersect(Jh.unk,Eh.unk,'rows');
P         = sparse(I1,I2,1,size(Jh.unk,1),size(Eh.unk,1));

% Sparse operators
tic
rotErotE = integral(omega,curl(Eh),curl(Eh));
EE       = integral(omega,Eh,Eh);
JE       = integral(sigma,Jh,Eh);
toc

% Full operators
tic
JTJ = 1/(4*pi) .* integral(gamma, gamma, Jh, Gxy, Jh,tol) ...
    - 1/(4*pi*k^2) * integral(gamma, gamma, div(Jh), Gxy, div(Jh),tol) ;
JTJ = JTJ + 1/(4*pi) * regularize(gamma, gamma, Jh, '[1/r]', Jh) ...
      - 1/(4*pi*k^2) * regularize(gamma, gamma, div(Jh), '[1/r]', div(Jh));
toc

tic
JKExn = - 1/(4*pi) * integral(gamma, sigma, Jh, Hxy, nx(Eh),tol);
JKExn = JKExn - 1/(4*pi) * regularize(gamma, gamma, Jh, 'grady[1/r]', Jh) * P;
toc

% LHS = [A B ; C D]
A = 1i*k *  JTJ;
size(A)

B = JKExn - 0.5 * JE;
size(B)

C = JE.';
size(C)

D = 1/(1i*k) * (rotErotE - k^2*EE);
size(D)

% Right hand side
Y   = - integral(gamma,Jh,PWE);
RHS = [Y;zeros(size(D,1),1)];

%%% SOLVE LINEAR PROBLEM
disp('~~~~~~~~~~~~~ SOLVE LINEAR PROBLEM ~~~~~~~~~~~~~')

% Factorization LU H-Matrix
tic
[La,Ua] = lu(A);
toc

figure
subplot(1,2,1)
spy(La)
subplot(1,2,2)
spy(Ua)

% Shurr complement resolution
tic
Sm1V = @(V) Ua\(La\V);
SV   = @(V) A*V - B*(D\(C*V));
J    = gmres(SV,Y,[],tol,100,Sm1V);
E    = - D\(C*J);
toc

%%% INFINITE SOLUTION
disp('~~~~~~~~~~~~~ INFINITE RADIATION ~~~~~~~~~~~~~')

% Plane waves direction
Ninf  = 1e3;
theta = 2*pi/1e3 .* (1:Ninf)';
nu    = [sin(theta),zeros(size(theta)),cos(theta)];

% Green kernel function
xdoty = @(X,Y) X(:,1).*Y(:,1) + X(:,2).*Y(:,2) + X(:,3).*Y(:,3);
Ginf  = @(X,Y) exp(-1i*k*xdoty(X,Y));

% Magnetic current radiation
Tinf = integral(nu,gamma,Ginf,Jh);
Jinf = 1i*k/(4*pi)*cross(nu, cross([Tinf{1}*J, Tinf{2}*J, Tinf{3}*J], nu));

% Electric field radiation
Kinf = integral(nu,sigma,Ginf,nx(Eh));
Einf = 1i*k/(4*pi) * cross(nu, [-Kinf{1}*E, -Kinf{2}*E, -Kinf{3}*E] );

% Total electric field radiation
sol = Jinf - Einf;

% Radiation infinie de reference, convention e^(+ikr)/r
nMax = 100; refInf = zeros(Ninf,1);
if E(1) == 1
    for jj = 1:Ninf
        refInf(jj,:) = sphereMaxwell(1, -f, theta(jj), 0.0, nMax);
    end
else
    for jj = 1:Ninf
        [~,refInf(jj)] = sphereMaxwell(1, -f, theta(jj), pi/2, nMax);
    end
end
refInf = refInf ./ sqrt(4*pi);

% Radiations infinies en X
if E(1) == 1
    sol = sin(theta)'.*sol(:,3) - cos(theta)'.*sol(:,1);
else
    sol = sol(:,2);
end

% Erreur
eL2   = norm(refInf-sol,2)/norm(refInf,2)
eLINF = norm(refInf-sol,'inf')/norm(refInf,'inf')

% Representation graphique
figure
subplot(1,2,1)
plot(theta,20*log10(abs(sol)),'b',theta,20*log10(abs(refInf)),'r--')

subplot(1,2,2)
plot(theta,real(sol),'--b', theta,imag(sol),'--r', theta, real(refInf),':b', theta,imag(refInf),':r');
drawnow

disp('~~> Michto gypsilab !')


function ComputingTopOpt_2Stage
% checkout: git checkout origin/master -- path/to/file
% PENDING: AUTOMATIZE THIS FUNCTION WITH GID PICTUREsss
rho0Name = 'TFM.mat';
jumpTo2ndPart = false;

if jumpTo2ndPart == false

%     fileName = 'jaCantilever';
%     % Data input
%     s.testName = [fileName,'.m'];
%     s.x1       = 2;
%     s.y1       = 1;
%     s.N        = 150;
%     s.M        = 75;
%     s.P        = -100;
%     s.DoF      = 2;
%
%     FEMWriter = FEMInputWriter(s);
%     FEMWriter.createTest;

    fileName = 'test_anisotropy_cantilever';
    s = Settings(fileName);
    s.warningHoleBC = false;
    s.printIncrementalIter = false;
    s.printChangingFilter = false;
    s.printing = false;
    translator = SettingsTranslator();
    translator.translate(s);
    fileName = translator.fileName;
    settings  = SettingsTopOptProblem(fileName);
    topOptSolver = TopOpt_Problem(settings);
    while topOptSolver.incrementalScheme.hasNext()
        topOptSolver.incrementalScheme.next();
        topOptSolver.optimizer.solveProblem();
    end

    rho0 = topOptSolver.designVariable.value;
    save(rho0Name,'rho0');

    % GiD Image Capturer:
%     outPutImageName = '"testgidpic"';
%     gidPath = '/home/joseantonio/GiDx64/gid-15.0.4/';
%     tclFile = 'callGiDCapturer.tcl';
%     tclFileTocall = 'CaptureImage3.tcl';
%     fid = fopen('/home/joseantonio/Documentos/GitHub/Swan/PostProcess/ImageCapturer/callGiDCapturer.tcl','w+');
%     fprintf(fid,['set path "/home/joseantonio/Documentos/GitHub/Swan/PostProcess/ImageCapturer','"\n']);
%     fprintf(fid,['set tclFile "',tclFileTocall,'"\n']);
%     fprintf(fid,['source $path$tclFile \n']);
%     fprintf(fid,['set output ',outPutImageName,' \n']);
%     fprintf(fid,['set inputFile "',fileName,'"\n']);
%     fprintf(fid,['CaptureImage $inputFile $output \n']);
%     fclose(fid);
%     command = [gidPath,'gid_offscreen -offscreen -t "source ',tclFile,'"'];
%     system(command);
%     inputImage  = [' ',outPutImageName,'.png'];
%     outPutImage = inputImage;
%     convert     = 'convert -crop 442x442+0+0 -gravity Center';
%     command = strcat(convert,' ',inputImage,' ',outPutImage);
%     system(command);
else
    load(rho0Name);
    fileName = 'test_anisotropy_cantilever_rho0';

    s = Settings(fileName);
    s.warningHoleBC = false;
    s.printIncrementalIter = false;
    s.printChangingFilter = false;
    s.printing = false;
    translator = SettingsTranslator();
    translator.translate(s);
    fileName = translator.fileName;
    settings  = SettingsTopOptProblem(fileName);
    DesignVariable = convertCharsToStrings(settings.designVarSettings.type);
    if  DesignVariable == "Density"
        settings.designVarSettings.creatorSettings.type = 'Given';
        settings.designVarSettings.creatorSettings.rho0 = rho0;
    elseif DesignVariable == "LevelSet"
        settings.designVarSettings.initialCase = 'given';
        settings.designVarSettings.creatorSettings.value = rho0;
    end
    topOptSolver = TopOpt_Problem(settings);
    while topOptSolver.incrementalScheme.hasNext()
        topOptSolver.incrementalScheme.next();
        topOptSolver.optimizer.solveProblem();
    end
end

end


classdef MOEAPSL < ALGORITHM
% <multi> <real/integer/binary> <large/none> <constrained/none> <sparse>
% Multi-objective evolutionary algorithm based on Pareto optimal subspace
% learning

%------------------------------- Reference --------------------------------
% Y. Tian, C. Lu, X. Zhang, K. C. Tan, and Y. Jin, Solving large-scale
% multi-objective optimization problems with sparse optimal solutions via
% unsupervised neural networks, IEEE Transactions on Cybernetics, 2021,
% 51(6): 3115-3128.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Population initialization
            P   = UniformPoint(Problem.N,Problem.D,'Latin');
            Dec = P.*repmat(Problem.upper-Problem.lower,Problem.N,1) + repmat(Problem.lower,Problem.N,1);
            Dec(:,Problem.encoding==4) = 1;
            Mask = UniformPoint(Problem.N,Problem.D,'Latin') > 0.5;
            Population = Problem.Evaluation(Dec.*Mask);
            [Population,Dec,Mask,FrontNo,CrowdDis] = EnvironmentalSelection(Population,Dec,Mask,Problem.N,0,0);

            %% Optimization
            rho = 0.5;
            while Algorithm.NotTerminated(Population)
                Site = rho > rand(1,ceil(Problem.N/2));
                if any(Site)
                    [rbm,dae,allZero,allOne] = ModelTraining(Mask,Dec,any(Problem.encoding~=4));
                else
                    [rbm,dae,allZero,allOne] = deal([]);
                end
                MatingPool = TournamentSelection(2,ceil(Problem.N/2)*2,FrontNo,-CrowdDis);
                [OffDec,OffMask] = Operator(Problem,Dec(MatingPool,:),Mask(MatingPool,:),rbm,dae,Site,allZero,allOne);
                Offspring = Problem.Evaluation(OffDec.*OffMask);
                [Population,Dec,Mask,FrontNo,CrowdDis,sRatio] = EnvironmentalSelection([Population,Offspring],[Dec;OffDec],[Mask;OffMask],Problem.N,length(Population),2*sum(Site));
                rho = (rho+sRatio)/2;
            end
        end
    end
end


%-------------------------------------------------------------------------------------------------------------
% This is an implementation of the TWSC algorithm for real-world image denoising
%
% Author:  Jun Xu, csjunxu@comp.polyu.edu.hk / nankaimathxujun@gmail.com
%          The Hong Kong Polytechnic University
%
% Please refer to the following paper if you find this code helps:
%
% @article{TWSC_ECCV2018,
% 	author = {Jun Xu and Lei Zhang and David Zhang},
% 	title = {A Trilateral Weighted Sparse Coding Scheme for Real-World Image Denoising},
% 	journal = {ECCV},
% 	year = {2018}
% }
%
% Please see the file License.txt for the license governing this code.
%-------------------------------------------------------------------------------------------------------------
clear;
GT_Original_image_dir = 'cc/';
GT_fpath = fullfile(GT_Original_image_dir, '*mean.png');
TT_Original_image_dir = 'cc/';
TT_fpath = fullfile(TT_Original_image_dir, '*real.png');
% GT_Original_image_dir = 'C:\Users\csjunxu\Desktop\CVPR2018 Denoising\PolyU\';
% GT_fpath = fullfile(GT_Original_image_dir, '*mean.JPG');
% TT_Original_image_dir = 'C:\Users\csjunxu\Desktop\CVPR2018 Denoising\PolyU\';
% TT_fpath = fullfile(TT_Original_image_dir, '*real.JPG');
GT_im_dir  = dir(GT_fpath);
TT_im_dir  = dir(TT_fpath);
im_num = length(TT_im_dir);

method = 'TWSC';
dataset = 'cc';
write_MAT_dir = [dataset '_Results/'];
write_sRGB_dir = [write_MAT_dir method];
if ~isdir(write_sRGB_dir)
    mkdir(write_sRGB_dir)
end

% Parameters
Par.ps = 6;        % patch size
Par.step = 3;       % the step of two neighbor patches
Par.win = 20;   % size of window around the patch
Par.Outerloop = 7;
Par.Innerloop = 2;
Par.maxIter = 10;
Par.maxrho = 100;
Par.nlspini = 70;
Par.display = 1;
Par.delta = 0;
Par.nlspgap = 0;
Par.lambda1 = 0;
Par.lambda2 = 4.9;

Par.nlspini = 70;
% set Parameters
% record all the results in each iteration
Par.PSNR = zeros(Par.Outerloop, im_num, 'double');
Par.SSIM = zeros(Par.Outerloop, im_num, 'double');
for i = 1 : im_num
    Par.nlsp = Par.nlspini;  % number of non-local patches
    Par.image = i;
    Par.nim = im2double(imread(fullfile(TT_Original_image_dir, TT_im_dir(i).name) ));
    Par.I = im2double(imread(fullfile(GT_Original_image_dir, GT_im_dir(i).name)));
    S = regexp(TT_im_dir(i).name, '\.', 'split');
    IMname = S{1};
    [h,w,ch] = size(Par.nim);
    % noise estimation
    for c = 1:ch
        Par.nSig(c) = NoiseEstimation(Par.nim(:, :, c)*255, Par.ps)/255;
    end
    % initial PSNR and SSIM
    fprintf('%s: \n', TT_im_dir(i).name);
    fprintf('The initial PSNR = %2.4f, SSIM = %2.4f. \n', csnr( Par.nim*255, Par.I*255, 0, 0 ), cal_ssim( Par.nim*255, Par.I*255, 0, 0 ));
    % denoising
    [IMout, Par]  =  TWSC_Sigma_RW(Par);
    % calculate the PSNR
    Par.PSNR(Par.Outerloop, Par.image)  =   csnr( IMout*255, Par.I*255, 0, 0 );
    Par.SSIM(Par.Outerloop, Par.image)      =  cal_ssim( IMout*255, Par.I*255, 0, 0 );
    %% output
    imwrite(IMout, [write_sRGB_dir '/' method '_' dataset '_' IMname '.png']);
    fprintf('%s : PSNR = %2.4f, SSIM = %2.4f \n',TT_im_dir(i).name, Par.PSNR(Par.Outerloop, Par.image),Par.SSIM(Par.Outerloop, Par.image)     );
end
PSNR =Par.PSNR(end,:);
SSIM = Par.SSIM(end,:);
mPSNR=mean(PSNR,2);
mSSIM=mean(SSIM,2);
matname = sprintf([write_MAT_dir method '_' dataset '.mat']);
save(matname,'PSNR','SSIM','mPSNR','mSSIM');


function offset = time2offset(time, fsample)

% TIME2OFFSET converts a time-axis of a trial into the offset in samples
% according to the definition from DEFINETRIAL
%
% Use as
%   [offset] = time2offset(time, fsample)
%
% The trialdefinition "trl" is an Nx3 matrix. The first column contains
% the sample-indices of the begin of the trial relative to the begin
% of the raw data , the second column contains the sample_indices of
% the end of the trials, and the third column contains the offset of
% the trigger with respect to the trial. An offset of 0 means that
% the first sample of the trial corresponds to the trigger. A positive
% offset indicates that the first sample is later than the triger, a
% negative offset indicates a trial beginning before the trigger.

% Copyright (C) 2005, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

offset = round(time(1)*fsample);


function T = argmin_g(w0, zeta, X, T)
     lhd= 1 ./  (w0 .^2 + zeta); % left hand

     % compute T for each channel
     for i = 1:size(X,3)
         T(:,:,i) = lhd .* X(:,:,i);
     end
end


function [hfig,finalim,final_pforefly] = OverlayFrames_Ellipses(trx,readframe,bkgdim0,predictions,mainfly,otherflies,ts,varargin)

colorpos = [.7,0,0];
colorneg = [0,0,.7];
border = 20;

[colorpos,colorneg,border,hfig,figpos,...
  cm,fg_thresh,bg_thresh,sigma_bkgd,wmah,frac_a_back,dist_epsilon,...
  ncolors_reference,solidbkgdcolor] = ...
  myparse(varargin,'colorpos',colorpos,...
  'colorneg',colorneg,...
  'border',border,...
  'hfig',[],...
  'figpos',[],...
  'colormap',{@jet,@jet},...
  'fg_thresh',115/255,...
  'bg_thresh',10/255,...
  'sigma_bkgd',115/255-10/255,...
  'wmah',.5,...
  'frac_a_back',1,...
  'dist_epsilon',.1,...
  'ncolors_reference',[],...
  'solidbkgdcolor',[.85,.85,.85]);

if isempty(otherflies),
  flygroups = {mainfly};
else
  flygroups = {mainfly,otherflies};
end
nflygroups = numel(flygroups);
nframes = numel(ts);
allts = ts(1):ts(end);
nframes_total = numel(allts);
allflies = [flygroups{:}];

[nr0,nc0,~] = size(bkgdim0);

%% grab out relevant time points
% not rotating currently, maybe rotate in the future?

x = nan(trx.nflies,nframes_total);
y = nan(trx.nflies,nframes_total);
a = nan(trx.nflies,nframes_total);
b = nan(trx.nflies,nframes_total);
theta = nan(trx.nflies,nframes_total);
imcenter = [(nc0+1)/2,(nr0+1)/2];
for fly = 1:trx.nflies,
  idx = allts+trx(fly).off;
  i0 = find(idx>1,1);
  i1 = find(idx<trx(fly).nframes,1,'last');
  x(fly,i0:i1) = trx(fly).x(idx(i0:i1));
  y(fly,i0:i1) = trx(fly).y(idx(i0:i1));
  a(fly,i0:i1) = trx(fly).a(idx(i0:i1));
  b(fly,i0:i1) = trx(fly).b(idx(i0:i1));
  theta(fly,i0:i1) = trx(fly).theta(idx(i0:i1));
end

bkgdim = bkgdim0;

%% figure out the axes limits
xlim = [inf,-inf];
ylim = [inf,-inf];
for fly = allflies,
  for j = 1:nframes_total,
    [x1,x2,y1,y2] = ellipse_to_bounding_box(x(fly,j),y(fly,j),a(fly,j),b(fly,j),theta(fly,j));
    xlim(1) = min([xlim(1),x1,x2]);
    xlim(2) = max([xlim(2),x1,x2]);
    ylim(1) = min([ylim(1),y1,y2]);
    ylim(2) = max([ylim(2),y1,y2]);
  end
end
xlim = [floor(xlim(1))-border,ceil(xlim(2))+border];
ylim = [floor(ylim(1))-border,ceil(ylim(2))+border];
bkgdim = bkgdim(ylim(1):ylim(2),xlim(1):xlim(2));
x = x - xlim(1)+1;
y = y - ylim(1)+1;

%% colors for each frame
colors_all = nan(ts(end)-ts(1)+1,3,nflygroups);
for i = 1:nflygroups,
  cmcurr = cm{min(numel(cm),i)};
  if isempty(cmcurr),
    colors_all(:,:,i) = 1;
  elseif isnumeric(cmcurr),
    colors_all(:,:,i) = cmcurr;
  elseif ~isempty(ncolors_reference),
    colors_reference = cmcurr(ncolors_reference);
    idxcurr = round(linspace(1,ncolors_reference,ts(end)-ts(1)+1));
    colors_all(:,:,i) = colors_reference(idxcurr,:);
  else
    colors_all(:,:,i) = cmcurr(ts(end)-ts(1)+1);
  end
end
colors = colors_all(ts-ts(1)+1,:,:);

%% read in the frames, convert to grayscale
imcurr = readframe(ts(1));
if size(imcurr,3) > 1,
  imcurr = rgb2gray(imcurr);
end
%imcurr = imtransform(imcurr,tform,'XData',[1,nc0],'YData',[1,nr0],'XYScale',1);
imcurr = imcurr(ylim(1):ylim(2),xlim(1):xlim(2));
gray_ims = im2double(imcurr);

%% probability that each pixel is foreground
dbkgd = abs(bsxfun(@minus,gray_ims,bkgdim));
pback = exp(-dbkgd/sigma_bkgd^2/2);
pback = pback / max(pback(:));
minv = exp(-fg_thresh/sigma_bkgd^2/2);
maxv = exp(-bg_thresh/sigma_bkgd^2/2);
pback = min(1,max(0,(pback-minv) / (maxv-minv)));
pfore = 1-pback;

%% make a connectivity graph for computing distances within foreground
% pixels
nr = ylim(2)-ylim(1)+1;
nc = xlim(2)-xlim(1)+1;
np = nr*nc;

[cgrid,rgrid] = meshgrid(1:nc,1:nr);
rconn1 = repmat(rgrid,[1,1,4]);
cconn1 = repmat(cgrid,[1,1,4]);
rconn2 = nan(size(rconn1));
cconn2 = nan(size(cconn1));

rconn2(:,:,1) = rgrid+1; % above
cconn2(:,:,1) = cgrid;
rconn2(:,:,2) = rgrid-1; % below
cconn2(:,:,2) = cgrid;
rconn2(:,:,3) = rgrid; % right
cconn2(:,:,3) = cgrid+1;
rconn2(:,:,4) = rgrid; % left
cconn2(:,:,4) = cgrid-1;
badidx = rconn2 < 1 | rconn2 > nr | cconn2 < 1 | cconn2 > nc;
rconn1(badidx) = [];
cconn1(badidx) = [];
rconn2(badidx) = [];
cconn2(badidx) = [];
conn1 = sub2ind([nr,nc],rconn1,cconn1);
conn2 = sub2ind([nr,nc],rconn2,cconn2);

%isdy = conn2 == conn1+1 | conn2 == conn1-1;
%w = min(maxdist,-log(pfore)+dist_epsilon)/maxdist;
w = reshape((pback+dist_epsilon)/(1+dist_epsilon),[np,1]);

%% assign pixels to flies

% probability that each pixel belongs to each fly
pfly = zeros(nr*nc,trx.nflies,1);
%allflies = [flygroups{:}];

ii = 1;
t = ts(ii);
i = t-ts(1)+1;
for fly = 1:trx.nflies,
  if t > trx(fly).endframe || t < trx(fly).firstframe,
    continue;
  end
  mu = [x(fly,i)-frac_a_back*a(fly,i)*cos(theta(fly,i)),...
    y(fly,i)-frac_a_back*a(fly,i)*sin(theta(fly,i))];
  %     mu = [trx(fly).x(j)-frac_a_back*trx(fly).a(j)*cos(trx(fly).theta(j)),...
  %       trx(fly).y(j)-frac_a_back*trx(fly).a(j)*sin(trx(fly).theta(j))];

  s = sub2ind([nr,nc],min(nr,max(1,round(mu(2)))),...
    min(nc,max(1,round(mu(1)))));
  wcurr = w(conn2,ii);
  S = axes2cov(a(fly,i),b(fly,i),theta(fly,i));
  diffs = bsxfun(@minus,[cgrid(:),rgrid(:)],mu);
  c = chol(S);
  dmah = sum((diffs/c).^2',1); %#ok<UDIM>
  %     ratio = S(1,1)/S(2,2);
  %     rationorm = 2 / (ratio+1);
  %     wcurr(isdy) = wcurr(isdy)*ratio*rationorm;
  %     wcurr(~isdy) = wcurr(~isdy)*rationorm;
  G = sparse(conn1,conn2,wcurr,np,np);
  d = graphshortestpath(G,s,'Directed',true);
  %pfly(:,fly,i) = exp(-d.^2/(trx(fly).a(j)*dist_epsilon)^2);
  pfly(:,fly,ii) = exp(-(1-wmah)*d.^2/(a(fly,i)*dist_epsilon)^2 - wmah*dmah);
end

Z = sum(pfly,2);
Z(Z==0) = 1;
pfly = bsxfun(@rdivide,pfly,Z);

gray_ims = reshape(gray_ims,[nr*nc,1]);
pfore = reshape(pfore,[nr*nc,1]);
psomefly = reshape(1-prod(1-pfly(:,[flygroups{:}],:),2),[nr*nc,1]);
pforefly = pfore.*psomefly;
%pfore_any = 1 - prod(1-pfore.*psomefly,2);

%% color the images

im = zeros(nr*nc,3,1);
i = 1;
imcurr = zeros(nr*nc,3);
for flygroupi = 1:nflygroups,
  flygroup = flygroups{flygroupi};
  pcurr = 1 - prod(1-pfly(:,flygroup,i),2);
  colorcurr = colors(i,:,flygroupi);
  imcurr = imcurr + bsxfun(@times,colorcurr,pcurr);
end
if ~isempty(solidbkgdcolor),
  imcurr = bsxfun(@plus,gray_ims(:,i).*pforefly(:,i),...
    bsxfun(@times,1-pforefly(:,i),repmat(reshape(solidbkgdcolor,[1,3]),[nr*nc,1])));
else
  imcurr = bsxfun(@plus,gray_ims(:,i).*pforefly(:,i),...
    (1-pforefly(:,i)).*reshape(bkgdim,[nr*nc,1]));
end
im(:,:,i) = imcurr;
finalim = reshape(im,[nr,nc,3]);

final_pforefly = reshape(pforefly,[nr,nc]);

%% plot

if isempty(hfig),
  hfig = figure;
end

figure(hfig);
clf;
if ~isempty(figpos),
  set(hfig,'Units','pixels','Position',figpos);
end
hax = gca;

idxpos = predictions{mainfly}(ts(1):ts(end));
timestamps = trx(mainfly).timestamps(ts+trx(mainfly).off)-...
  trx(mainfly).timestamps(ts(1)+trx(mainfly).off);

i = 1;
t = ts(i);
image(finalim(:,:,:,i),'Parent',hax(i));
axis(hax(i),'image','off');
hold(hax(i),'on');
plot(hax(i),x(mainfly,:),y(mainfly,:),'k.-');
plot(hax(i),x(mainfly,idxpos),y(mainfly,idxpos),'.','Color',colorpos);
plot(hax(i),x(mainfly,~idxpos),y(mainfly,~idxpos),'.','Color',colorneg);
if predictions{mainfly}(t),
  colorcurr = colorpos;
else
  colorcurr = colorneg;
end
plot(hax(i),x(mainfly,t-ts(1)+1),y(mainfly,t-ts(1)+1),'o','color',colorcurr,'markerfacecolor',colorcurr);
text(1,1,sprintf('t = %.2fs',timestamps(i)),'HorizontalAlignment','left','VerticalAlignment','top','Parent',hax(i));

hell = nan(1,nframes);
for i = 1:nframes,
  j = ts(i)-ts(1)+1;
  hell(i) = drawellipse(x(mainfly,j),y(mainfly,j),theta(mainfly,j),2*a(mainfly,j),2*b(mainfly,j));
  set(hell(i),'LineWidth',2,'Color',colors(i,:,1));
end

colormap(colors_all(:,:,1));
tlim = timestamps(end);
set(hax,'CLim',[0,tlim]);
hcb = colorbar('East');
tticks = [0,tlim];
realylim = get(hcb,'YLim');
yticks = realylim(1)+(tticks/tlim)*(realylim(2)-realylim(1));
set(hcb,'YTick',yticks,'YTickLabel',tticks');

if isempty(figpos),
  truesize;
else
  set(hfig,'Units','pixels','Position',figpos);
end


%%***************************************************************
%% linsysolve: solve linear system to get dy, and direction
%%             corresponding to unrestricted variables.
%%
%% [xx,coeff,L,resnrm] = linsysolve(schur,UU,EE,Bmat,rhs);
%%
%% child functions: mybicgstable.m
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%***************************************************************

   function [xx,coeff,L,resnrm] = HSDlinsysolve(par,schur,UU,EE,Bmat,rhs);

    global solve_ok msg
    global nnzmat nnzmatold matfct_options matfct_options_old use_LU
    global Lsymb

    spdensity  = par.spdensity;
    printlevel = par.printlevel;
    iter       = par.iter;

    m = length(schur);
    if (iter==1); use_LU = 0; matfct_options_old = ''; end
    if isempty(nnzmatold); nnzmatold = 0; end
%%
%% diagonal perturbation
%% old: pertdiag = 1e-15*max(1,diagschur);
%%
    diagschur = abs(full(diag(schur)));
    const = 1e-2/max(1,norm(par.dy2));
    alpha = max(1e-14,min(1e-10,const*norm(par.rp))/(1+norm(diagschur.*par.dy2)));
    pertdiag = alpha*max(1e-8,diagschur); %% Note: alpha is close to 1e-15.
    mexschurfun(schur,pertdiag);
    %%if (printlevel); fprintf(' %3.1e ',alpha); end
    if (par.depconstr) | (min(diagschur) < min([1e-20*max(diagschur), 1e-4]))
       lambda = 0.1*min(1e-14,const*norm(par.rp)/(1+norm(par.diagAAt.*par.dy2)));
       mexschurfun(schur,lambda*par.diagAAt);
       if (printlevel); fprintf('#'); end
    end
    if (max(diagschur)/min(diagschur) > 1e14) & (par.blkdim(2) == 0) ...
       & (iter > 10)
       tol = 1e-6;
       idx = find(diagschur < tol); len = length(idx);
       pertdiagschur = zeros(m,1);
       if (len > 0 & len < 5) & (norm(rhs(idx)) < tol)
          pertdiagschur(idx) = 1*ones(length(idx),1);
          mexschurfun(schur,pertdiagschur);
          if (printlevel); fprintf('$'); end
       end
    end
%%
%%
%%
    UU = [UU, Bmat];
    if ~isempty(EE)
       len = max(max(EE(:,1)),max(EE(:,2)));
    else
       len = 0;
    end
    tmp = [len+1,len+3,-1; len+2,len+4,1; len+3,len+1,1; len+4,len+2,-1;
           len+2,len+2,par.addschur];  %% this is the -inverse
    EE = [EE; tmp];
    ncolU = size(UU,2);
%%
%% assemble coefficient matrix
%%
    if isempty(EE)
       coeff.mat22 = [];
    else
       coeff.mat22 = spconvert(EE);
    end
    coeff.mat12 = UU;
    coeff.mat11 = schur; %% important to use perturbed schur matrix
%%
%% pad rhs with zero vector
%% decide which solution methods to use
%%
    rhs = [rhs; zeros(m+ncolU-length(rhs),1)];
    if (ncolU > 300); use_LU = 1; end
%%
%% Cholesky factorization
%%
    L = []; resnrm = []; xx = inf*ones(m,1);
    if (~use_LU)
       nnzmat = mexnnz(coeff.mat11);
       nnzmatdiff = (nnzmat ~= nnzmatold);
       solve_ok = 1;  solvesys = 1;
       if (nnzmat > spdensity*m^2) | (m < 500)
          matfct_options = 'chol';
       else
          matfct_options = 'spchol';
       end
       if (printlevel > 2); fprintf(' %s',matfct_options); end
       if strcmp(matfct_options,'chol')
          if issparse(schur); schur = full(schur); end;
          if (iter<=5); %%--- to fix strange anonmaly in Matlab
             mexschurfun(schur,1e-20,2);
          end
          L.matfct_options = 'chol';
          [L.R,indef] = chol(schur);
          L.perm = [1:m];
      elseif strcmp(matfct_options,'spchol')
          if ~issparse(schur); schur = sparse(schur); end;
          L.matfct_options = 'spchol';
          [L.R,indef,L.perm] = chol(schur,'vector');
          L.Rt = L.R';
       end
       if (indef)
          solve_ok = -2; solvesys = 0;
          msg = 'HSDlinsysolve: Schur complement matrix not positive definite';
          if (printlevel); fprintf('\n  %s',msg); end
       end
       if (solvesys)
          if (ncolU)
             tmp = coeff.mat12'*linsysolvefun(L,coeff.mat12)-coeff.mat22;
	     if issparse(tmp); tmp = full(tmp); end
             [L.Ml,L.Mu,L.Mp] = lu(tmp);
             tol = 1e-16;
             condest = max(abs(diag(L.Mu)))/min(abs(diag(L.Mu)));
             idx = find(abs(diag(L.Mu)) < tol);
             if ~isempty(idx) | (condest > 1e30*sqrt(norm(par.diagAAt))); %% default=1e25
                solvesys = 0; solve_ok = -4;
                use_LU = 1;
                msg = 'SMW too ill-conditioned, switch to LDL factor';
                if (printlevel); fprintf('\n  %s, %2.1e.',msg,condest); end
             end
          end
          if (solvesys)
             [xx,resnrm,solve_ok] = HSDbicgstab(coeff,rhs,L,[],[],printlevel);
             if (solve_ok<=0) & (printlevel)
                fprintf('\n  warning: HSDbicgstab fails: %3.1f.',solve_ok);
             end
          end
       end
       if (solve_ok < 0)
          if (m < 6000 & strcmp(matfct_options,'chol')) | ...
             (m < 1e5 & strcmp(matfct_options,'spchol'))
             use_LU = 1;
             if (printlevel); fprintf('\n  switch to LDL factor'); end
          end
       end
    end
%%
%% symmetric indefinite LDL factorization
%%
    if (use_LU)
       nnzmat = mexnnz(coeff.mat11)+mexnnz(coeff.mat12);
       nnzmatdiff = (nnzmat ~= nnzmatold);
       solve_ok = 1;
       if ~isempty(coeff.mat22)
          raugmat = [coeff.mat11, coeff.mat12; coeff.mat12', coeff.mat22];
       else
          raugmat = coeff.mat11;
       end
       if (nnzmat > spdensity*m^2) | (m+ncolU < 500)
          matfct_options = 'ldl';
       else
          matfct_options = 'spldl';
       end
       if (printlevel > 2); fprintf(' %s ',matfct_options); end
       if strcmp(matfct_options,'ldl')
          if issparse(raugmat); raugmat = full(raugmat); end
          L.matfct_options = 'ldl';
          [L.L,L.D] = ldl(raugmat);
          L.perm = [1:length(raugmat)]';
       elseif strcmp(matfct_options,'spldl')
          if ~issparse(raugmat); raugmat = sparse(raugmat); end
          L.matfct_options = 'spldl';
          [L.L,L.D,L.perm] = ldl(raugmat,'vector');
          L.Lt = L.L';
       end
       [xx,resnrm,solve_ok] = HSDbicgstab(coeff,rhs,L,[],[],printlevel);
       if (solve_ok<=0) & (printlevel)
          fprintf('\n  warning: HSDbicgstab fails: %3.1f,',solve_ok);
       end
    end
    if (printlevel>=3); fprintf('%2.0f ',length(resnrm)-1); end
%%
    nnzmatold = nnzmat; matfct_options_old = matfct_options;
%%***************************************************************


function parameters = convert_ma_roots(parameters, q)
% Checks an MA parameterization for invertability and inverts it if possible
%
%
%
% COMMENTS:
% It is not always possible to invert irregular MAs since the indices of the non-zero coefficients
% in the invertable MA may differ.  If this is the case PARAMETERS will be equal to the input.

MAparameters = zeros(1,max(q));
MAparameters(q) = parameters;
lambda = roots([1 MAparameters]);
lambda(abs(lambda)>1) = 1./lambda(abs(lambda)>1);
parameters = poly(lambda);
parameters(abs(parameters)<1e-10) = 0;

if length(q)<max(q)
    % Check for index change
    newq = find(abs(parameters)>1e-10);
    newq = newq(2:length(newq))-1;
    if length(q)~=length(newq) || any(q~=newq)
        warning('MFEToolbox:Invertability',['The irregular MA cannot be inverted without changing the lag indices. \nThe require lag indices are:' num2str(newq)])
        parameters = MAparameters(q);
    else
        parameters = parameters(q+1);
    end
else
    parameters = parameters(q+1);
end

if size(parameters,2)>size(parameters,1)
    parameters = parameters';
end


% Copyright 2011 Zdenek Kalal
%
% This file is part of TLD.
%
% TLD 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.
%
% TLD 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 TLD.  If not, see <http://www.gnu.org/licenses/>.

function [BB2 Conf Valid tld] = tldTrack_occlusion(tld,BB1,I,J)

GRID      = tld.tracker.grid;
BIG_FB    = tld.tracker.big_fb;
% OCCLUSION = tld.tracker.occlusion;

BB2    = [];
Conf   = [];
Valid  = 0;

if isempty(BB1) || ~bb_isdef(BB1),
    return;
end

% Forward
xFI    = bb_points(BB1,GRID,GRID,5);
xFJ    = lk(2,tld.img{I}.input,tld.img{J}.input,xFI,xFI,3);
idxFB  = xFJ(3,:) < 2;
idxNCC = xFJ(4,:) > 0.9;

% Occlusion detection
 xF1    = bb_points(tld.bb(:,1),GRID,GRID,5);
 xB1    = lk(2,tld.img{I}.input,tld.img{1}.input,xFI,xFI,1);
 idxOCC = xB1(3,:) < .1 & xB1(4,:) > 0.95 & vnormp(xF1(1:2,:)-xB1(1:2,:),2) > 10;

xFI    = xFI(:,~idxOCC & idxFB & idxNCC);
xFJ    = xFJ(:,~idxOCC & idxFB & idxNCC);

medFB  = median2(xFJ(3,:));
medNCC = median2(xFJ(4,:));
idxF   = xFJ(3,:) <= medFB & xFJ(4,:)>= medNCC;
BB2    = bb_predict(BB1,xFI(:,idxF),xFJ(1:2,idxF));

% Failure detection
if medFB > BIG_FB || ~bb_isdef(BB2) || bb_isout(BB2,tld.imgsize)
    disp('Tracker uncertain.');
    BB2 = [];
    return;
end

% Output
ex       = tldGetPattern(tld.img{J},BB2,tld.model.patchsize);
[~,Conf] = tldNN(ex,tld);
Valid    = tld.valid(I);

% if sum(idxOCC)/length(idxOCC) >= .20
%     tld.trackerfailure(J) = 1;
%     disp('Tracker occlusion.');
%     %disp('tracker failure');
%     Valid = 0;
% end

% Match error
% resF  = mean(vnormp(xFJ(1:2,idxF)-xBJ(:,idxF),2)); % residual of forward match
% resB  = mean(vnormp(xBI(1:2,idxB)-xFI(:,idxB),2)); % residual of backward match
% %BB_shift = vnormp(BB_shift,2);
% error = abs(resF-resB);

% if ~bb_isin(BB2,tld.imgsize) || error > OCCLUSION
%     Valid = 0;
% end


function tests = test_imEquivalentEllipsoid
% Test suite for the file imEquivalentEllipsoid.
%
%   Test suite for the file imEquivalentEllipsoid
%
%   Example
%   test_imEquivalentEllipsoid
%
%   See also
%     imEquivalentEllipsoid

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2021-11-03,    using Matlab 9.10.0.1684407 (R2021a) Update 3
% Copyright 2021 INRAE - BIA-BIBS.

tests = functiontests(localfunctions);

function test_Simple(testCase) %#ok<*DEFNU>
% Test call of function without argument.

%
img = readstack(fullfile('files', 'ellipsoid_Center30x27x25_Size20x12x8_Orient40x30x20.tif'));

elli = imEquivalentEllipsoid(img > 0);

assertEqual(testCase, size(elli), [1 9]);
assertEqual(testCase, [30 27 25], elli(1:3), 'AbsTol', 0.5);
assertEqual(testCase, [20 12  8], elli(4:6), 'AbsTol', 0.5);
assertEqual(testCase, [40 30 20], elli(7:9), 'AbsTol', 0.5);

function test_Calibrated(testCase) %#ok<*DEFNU>
% Test call of function without argument.

img = readstack(fullfile('files', 'ellipsoid_Center30x27x25_Size20x12x8_Orient40x30x20.tif'));

elli = imEquivalentEllipsoid(img > 0, 'Spacing', [0.5 0.5 0.5], 'Origin', [0.5 0.5 0.5]);

assertEqual(testCase, size(elli), [1 9]);
assertEqual(testCase, [15 13.5 12.5], elli(1:3), 'AbsTol', 0.5);
assertEqual(testCase, [10 6 4], elli(4:6), 'AbsTol', 0.5);
assertEqual(testCase, [40 30 20], elli(7:9), 'AbsTol', 0.5);


function cs_demo (do_pause, matrixpath)
%CS_DEMO run all CXSparse demos.
%   cs_demo(0) will run all demos without pausing.
%
% Example:
%   cs_demo
% See also: cs_demo1, cs_demo2, cs_demo3

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

help cs_demo
if (nargin < 1)
    do_pause = 1 ;
end
if (nargin < 2)
    matrixpath = [] ;
end

figure (1)
clf

fprintf ('\n\n-------------------------------------------------------\n') ;
help cs_demo1 ;
cs_demo1 (matrixpath) ;

fprintf ('\n\n-------------------------------------------------------\n') ;
help cs_demo2
cs_demo2 (do_pause, matrixpath) ;

fprintf ('\n\n-------------------------------------------------------\n') ;
help cs_demo3
cs_demo3 (do_pause, matrixpath) ;

fprintf ('\n\n-------------------------------------------------------\n') ;
help private/ex_1
ex_1

fprintf ('\n\n-------------------------------------------------------\n') ;
help private/ex2
ex2

fprintf ('\n\n-------------------------------------------------------\n') ;
help private/ex3
ex3

fprintf ('\nAll CXSparse demos finished.\n') ;


function imsegs = processSuperpixelImage(fn)
% imsegs = processSuperpixelImage(fn)
% Creates the imsegs structure from a segmentation image
%
% INPUT:
% fn - filenames of segmentation images. Use '/' (not '\') to separate directories.
% Segments are denoted by different RGB colors.
%
% OUTPUT:
% imsegs - image segmentation data
%
% Copyright(C) Derek Hoiem, Carnegie Mellon University, 2006

if isstr(fn)
    fn = {fn};
end

imsegs(length(fn)) = struct('imname', '', 'imsize', [0 0]);
for f = 1:length(fn)
    toks = strtokAll(fn{f}, '/');
    imname = toks{end};
    basename = strtok(imname, '.');
    im = imread(fn{f});
    im = double(im);

    imsegs(f).imname = [strtok(imname,'.') '.jpg'];
    imsegs(f).imsize = size(im);
    imsegs(f).imsize = imsegs(f).imsize(1:2);
    im = im(:, :, 1) + im(:, :, 2)*256 + im(:, :, 3)*256^2;
    [gid, gn] = grp2idx(im(:));
    imsegs(f).segimage = uint16(reshape(gid, imsegs(f).imsize));
    imsegs(f).nseg = length(gn);
end
imsegs = APPgetSpStats(imsegs);


classdef refractiveIndexTensor < tensor

  methods

    function rI = refractiveIndexTensor(varargin)
      rI = rI@tensor(varargin{:},'rank',2);

      % TODO: set up the unit correctly !!!

    end

  end

  methods (Static = true)
    function rI = calcite
      cs = crystalSymmetry('-3m1',[5,5,17],'mineral','Calcite','X||a');
      rI = refractiveIndexTensor(diag([1.66 1.66 1.486]),cs);
    end

    function rI = olivin
      cs = loadCIF('olivin.cif');
      rI = refractiveIndexTensor(diag([1.640 1.660 1.680]),cs);
    end

    function test
      rI = refractiveIndexTensor.calcite;

      vprop = plotS2Grid;

      figure(1)
      n = rI.birefringence(vprop);

      plot3d(vprop,n)

      figure(2)
      thickness = 10000;
      rgb = spectralTransmission(rI,vprop,thickness);
      plot3d(vprop,rgb./100)

    end

    function test2
      mtexdata fo

      rI = refractiveIndexTensor.olivin;

      oM = spectralTransmissionColorKey(rI,1000);
      oM.polarizer = vector3d.Y;

      plot(ebsd('fo'),oM.orientation2color(ebsd('fo').orientations)./100)

    end

  end

end


function [spos_n, svel_b, sCsn, simu]=la_imu(acc, gyro, gyro_der, pos_n, vel_b, Cbn, imu_la, imu_or, typ, gyro_vib, imu_vib)

nimu=size(imu_la,2);
spos_n=zeros(3,nimu);
svel_b=zeros(3,nimu);
sCsn=zeros(3,3,nimu);
simu=zeros(6,nimu);

%vibration information is not compulsory
if (isempty(gyro_vib))
    gyro_vib=zeros(3,nimu);
end

if (isempty(imu_vib))
    imu_vib=zeros(3,3,nimu);
    for in=1:nimu
        imu_vib(:,:,in)=eye(3);
    end
end

if (typ==0)
    [Rn, Re, g, sL, cL, WIE]=geo_param(pos_n);
end
for in=1:nimu
    %Attitude
    Crig=imu_or(:,:,in);
    Cvib=imu_vib(:,:,in);
    Csm=Crig*Cvib;
    Csn=Cbn*Csm;
    sCsn(:,:,in)=Csn;

    %position
    la_m=imu_la(:,in);
    if (typ==0)
        spos_n(:,in)=pos_n+[1/(Rn+pos_n(3)) 0 0;0 1/cL/(Re+pos_n(3)) 0; 0 0 -1]*Cbn*la_m;
    elseif (typ==1)
        spos_n(:,in)=pos_n+Cbn*la_m;
    end

    %Velocity
    if (typ==0)
        wie_m=Cbn'*[WIE*cL; 0; -WIE*sL];
    else
        wie_m=zeros(3,1);
    end
    svel_b(:,in)=Csm'*(vel_b+cross(gyro-wie_m,la_m));

    %Acceleration
    simu(1:3,in)=Csm'*(acc+cross(gyro,cross(gyro,la_m))+cross(gyro_der,la_m));

    %Rotation rate
    simu(4:6,in)=Csm'*(gyro+gyro_vib(:,in));
end


function [data,mask,test,masktest]= genadding(problem)
global   mzeros convert
data=mzeros(problem.numsamples, 4 , ceil(problem.T*1.1));
mask=mzeros(problem.numsamples, 1 , ceil(problem.T*1.1));
test=mzeros(problem.numtest, 4 , ceil(problem.Ttest*1.1));
masktest=mzeros(problem.numtest, 1 , ceil(problem.Ttest*1.1));

T=fix(problem.T/1.1);
for i = 1 : problem.numsamples

    length = T+ fix(rand  * T / 10) ;
    mask(i,1,length)=1;

    data(i,3,1:length)=ones(1, length); %bias
    a=2*rand(1,length    ) - 1;
    data(i,1,1:length)=a;
    a=a';

    b=zeros(length,1);  %flag
    c=20*ones(length,1);

    data(i,4,1:length)=ones(1,length);%bias

    if T>10
        tmp1= ceil( rand * 10);
    else
        tmp1= ceil( rand * T/2);
    end

    data(i,2,tmp1)=1;
    b(tmp1,1)=1;

    tmp2=tmp1;
    while tmp2 == tmp1
        tmp2=  ceil(rand * T/2);
    end

    data(i,2,tmp2) = 1;
    b(tmp2,1)=1;

    if tmp2 ==1  || tmp1 ==1
        data(i,2,1)=0;
        b(1,1)=0;
    else
        data(i,2,1)=-1;
        b(1,1)=-1;
    end

    data(i,2,length-1)=-1;
    b(end,1)=-1;
    data(i,1,length) = 0.5+(data(i,1,tmp1)+data(i,1,tmp2))/4;
    c(end,1)= 0.5+(a(tmp1,1)+a(tmp2,1))/4;

end

for i =   size(data,3) :-1:1
    if max( max( data(:,:,i))) ~=0
        data=data(:,:,1:i);
        break
    end
end

for i = 1 : problem.numtest

    length = T+ fix(rand  * T / 10) ;
    masktest(i,1,length)=1;

    test(i,3,1:length)=ones(1, length); %bias
    test(i,1,1:length)=2*rand(1,length    ) - 1;

    test(i,4,1:length) = ones(1,length);%bias

    if T>10
        tmp1= ceil( rand * 10);
    else
        tmp1= ceil( rand * T/2);
    end

    test(i,2,tmp1)=1;

    tmp2=tmp1;
    while tmp2 == tmp1
        tmp2=  ceil(rand * T/2);
    end

    test(i,2,tmp2) = 1;

    if tmp2 ==1  || tmp1 ==1
        test(i,2,1)=0;
    else
        test(i,2,1)=-1;
    end

    test(i,2,length-1)=-1;
    test(i,1,length) = 0.5+(test(i,1,tmp1)+test(i,1,tmp2))/4;
end
for i =   size(test,3) :-1:1
    if max( max( test(:,:,i))) ~=0
        test=test(:,:,1:i);
        break
    end
end

tmp=test;
test=mzeros(size(test,1) ,  size(test,2) ,  size(test,3) +1  );
test(:,:,1)=tmp(:,:,1);
test(:,:,2:end)=tmp;

masktest=masktest(:,:,1:size(test,3));
tmp=masktest;
masktest=mzeros(size(masktest,1) ,  size(masktest,2) ,  size(masktest,3) +1  );
masktest(:,:,1)=tmp(:,:,1);
masktest(:,:,2:end)=tmp;

data=convert(data);
mask=convert(mask);
test=convert(test);
masktest=convert(masktest);


%  lyapunov_equation solves the equation V=T*V*T'+Q
%
%  ::
%
%    [V,retcode]=lyapunov_equation(T,Q)
%    [V,retcode]=lyapunov_equation(T,Q,options)
%
%  Args:
%     - T :
%     - Q :
%     - options :
%
%  Returns:
%     :
%     - V :
%     - retcode :
%
%  Note:
%
%  Example:
%
%     See also:
%


function op = linop_compose( varargin )
%LINOP_COMPOSE Composes two TFOCS linear operators
%OP = LINOP_COMPOSE( OP1, OP2, ..., OPN )
%    Constructs a TFOCS-compatible linear operator from the composition of
%    two or more linear operators and/or matrices. That is,
%       OP(x,1) = OP1(OP2(...(OPN(x,1),...,1),1)
%       OP(x,2) = OPN(...(OP2(OP1(x,2),2)...,2)
%    If matrices are supplied, they must be real; to include complex
%    matrices, convert them first to linear operators using LINOP_MATRIX.

if nargin == 0,
    error( 'Not enough input arguments.' );
end
sz = { [], [] };
for k = 1 : nargin,
    tL = varargin{k};
    if isempty(tL) || ~isa(tL,'function_handle') && ~isnumeric(tL) || ndims(tL) > 2,
        error( 'Arguments must be linear operators, scalars, or matrices.' );
    elseif isnumeric(tL),
        if ~isreal(tL),
            error( 'S or scalar arguments must be real.' );
        elseif numel(tL) == 1,
            tL = linop_scale( tL );
        else
            tL = linop_matrix( tL );
        end
        varargin{k} = tL;
    end
    try
        tsz = tL([],0);
    catch
        error( 'Arguments must be linear operators, scalars, or matrices.' );
    end
    if isempty(tsz)     % i.e. output of linop_identity is []
        tsz = { [], [] };
    end
    if isnumeric(tsz),
        tsz = { [tsz(2),1], [tsz(1),1] };
    end

    % convert [n1;n2] to [n1,n2] if necessary:
    for kk = 1:2
        %if iscolumn( tsz{kk} )
            %tsz{kk} = tsz{kk}.';
        %end
        tsz{kk} = tsz{kk}(:).';
    end

    if ~isempty(sz{1}) && ~isempty(tsz{2}) && ~isequal(tsz{2},sz{1}),
        for kk = 1:min( numel(tsz{2}), numel( sz{1} ) )
            fprintf('Found incompatible sizes: %d ~= %d\n', tsz{2}(kk), sz{1}(kk) );
        end
        error( 'Incompatible dimensions in linear operator composition.' );
    end
    if ~isempty(tsz{1}),
        sz{1} = tsz{1};
    end
    if isempty(sz{2}),
        sz{2} = tsz{2};
    end
end
% Explanation of above code:
% suppose have three inputs, opA, opB, opC; with sizes szA, szB, szC
%   where opA: szA{1} --> szA{2}
%   so we need szC{2} == szB{1} and szB{2} = szA{1}
%
% so  sz{2} = szA{2}
% and sz{1} = szC{1}

if nargin == 1,
    op = varargin{1};
else
    op = @(x,mode)linop_compose_impl( varargin, sz, x, mode );
end

function y = linop_compose_impl( ops, sz, y, mode )
switch mode,
    case 0,
        y = sz;
    case 1,
        for k = numel(ops) : -1 : 1,
            y = ops{k}( y, 1 );
        end
    case 2,
        for k = 1 : numel(ops),
            y = ops{k}( y, 2 );
        end
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function adj = tet_mesh_order4_adj_set ( node_num, tetra_num, tetra_node, ...
  adj_num, adj_row )

%*****************************************************************************80
%
%% TET_MESH_ORDER4_ADJ_SET sets the nodal adjacency matrix.
%
%  Discussion:
%
%    A compressed format is used for the nodal adjacency matrix.
%
%    It is assumed that we know ADJ_NUM, the number of adjacency entries
%    and the ADJ_ROW array, which keeps track of the list of slots
%    in ADJ where we can store adjacency information for each row.
%
%    We essentially repeat the work of TET_MESH_ORDER4_ADJ_COUNT, but
%    now we have a place to store the adjacency information.
%
%    A copy of the ADJ_ROW array is useful, as we can use it to keep track
%    of the next available entry in ADJ for adjacencies associated with
%    a given row.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer TETRA_NUM, the number of tetrahedrons.
%
%    Input, integer TETRA_NODE(4,TETRA_NUM), the nodes that make up the
%    tetrahedrons.
%
%    Input, integer ADJ_NUM, the total number of adjacency relationships,
%
%    Input, integer ADJ_ROW(NODE_NUM+1), the ADJ pointer array.
%
%    Output, integer ADJ(ADJ_NUM), the adjacency information.
%

%
%  Each order 4 tetrahedron defines 6 adjacency pairs.
%
  pair(1,            1:  tetra_num) = tetra_node(1,1:tetra_num);
  pair(2,            1:  tetra_num) = tetra_node(2,1:tetra_num);

  pair(1,  tetra_num+1:2*tetra_num) = tetra_node(1,1:tetra_num);
  pair(2,  tetra_num+1:2*tetra_num) = tetra_node(3,1:tetra_num);

  pair(1,2*tetra_num+1:3*tetra_num) = tetra_node(1,1:tetra_num);
  pair(2,2*tetra_num+1:3*tetra_num) = tetra_node(4,1:tetra_num);

  pair(1,3*tetra_num+1:4*tetra_num) = tetra_node(2,1:tetra_num);
  pair(2,3*tetra_num+1:4*tetra_num) = tetra_node(3,1:tetra_num);

  pair(1,4*tetra_num+1:5*tetra_num) = tetra_node(2,1:tetra_num);
  pair(2,4*tetra_num+1:5*tetra_num) = tetra_node(4,1:tetra_num);

  pair(1,5*tetra_num+1:6*tetra_num) = tetra_node(3,1:tetra_num);
  pair(2,5*tetra_num+1:6*tetra_num) = tetra_node(4,1:tetra_num);

  pair_num = 6 * tetra_num;
%
%  Force the nodes of each pair to be listed in ascending order.
%
  pair = i4col_sort2_a ( 2, pair_num, pair );
%
%  Rearrange the columns in ascending order.
%
  pair = i4col_sort_a ( 2, pair_num, pair );
%
%  Mark all entries of ADJ so we will know later if we missed one.
%
  adj(1:adj_num) = -1;
%
%  Copy the ADJ_ROW array and use it to keep track of the next
%  free entry for each row.
%
  adj_row_copy(1:node_num) = adj_row(1:node_num);
%
%  Now set up the ADJ_ROW counts.
%
  for k = 1 : pair_num

    if ( 1 < k )
      if ( pair(1,k-1) == pair(1,k) && pair(2,k-1) == pair(2,k) )
        continue
      end
    end

    i = pair(1,k);
    j = pair(2,k);

    adj(adj_row_copy(i)) = j;
    adj_row_copy(i) = adj_row_copy(i) + 1;
    adj(adj_row_copy(j)) = i;
    adj_row_copy(j) = adj_row_copy(j) + 1;

  end

  return
end


function projections = findProjections(filePath,vecs,meanValues,pixels,parameters)
%findPosturalEigenmodes finds the projection of a set of images onto
%postural eigenmodes.
%
%   Input variables:
%
%       filePath -> cell array of VideoReader objects or a directory
%                       containing aligned .avi files
%       vecs -> postural eignmodes (L x (M<L) array)
%       meanValues -> mean value for each of the pixels
%       pixels -> radon-transform space pixels to use (Lx1 or 1xL array)
%       parameters -> struct containing non-default choices for parameters
%
%
%   Output variables:
%
%       projections -> N x d array of projection values
%
%
% (C) Gordon J. Berman, 2014
%     Princeton University

    if nargin < 5
        parameters = [];
    end
    parameters = setRunParameters(parameters);

    setup_parpool(parameters.numProcessors)

    %files = findAllImagesInFolders(filePath,'tiff');
    %N = length(files);

    if iscell(filePath)

        vidObjs = filePath;

    else

        files = findAllImagesInFolders(filePath,'avi');
        N = length(files);
        vidObjs = cell(N,1);
        parfor i=1:N
           vidObjs{i} = VideoReader(files{i});
        end

    end

    numThetas = parameters.num_Radon_Thetas;
    spacing = 180/numThetas;
    thetas = linspace(0,180-spacing,numThetas);
    scale = parameters.rescaleSize;
    numProjections = parameters.numProjections;
    batchSize = parameters.pca_batchSize;

    projections = find_PCA_projections(vidObjs,vecs(:,1:numProjections),...
        meanValues,pixels,thetas,numProjections,scale,batchSize);

    if parameters.numProcessors > 1  && parameters.closeMatPool
        close_parpool
    end


function A = spm_fnirs_sensitivity(DCM)
% Calculate sensitivity matrix which corresponds to the effective
% pathlength of detected photons for the channel measurements in the
% hemodynamic source.
% FORMAT [A] = spm_fnirs_sensitivity(DCM)
%
% DCM - DCM structure or its filename
%
% A - sensitivity matrix
%
% Green's function (see \dcm_fnirs\mmclab\estimate_greens_mmclab.m)
%--------------------------------------------------------------------------
% G.s - estimated Green's function from sensor (light emitter) positions
% into source positions [# sensor x # voxels x # wavelengths]
% G.d - estimated Green's function from sensor (light detector) positions
% into source positions [# sensor x # voxels x # wavelengths]
% G.xyz - MNI locations [3 x # voxels]
% G.elem - tissue types of voxels [3 x # voxels]
% 1-scalp, 2-CSF, 3-gray matter, 4-white matter
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny & Sungho Tak
% $id$

%--------------------------------------------------------------------------
if ~nargin || isempty(DCM)
    [DCM, sts] = spm_select(1, '^DCM*.*\.mat$', 'Select DCM.mat');
    if ~sts, S = []; return; end
elseif ischar(DCM)
    load(DCM);
end

%--------------------------------------------------------------------------
load(DCM.Y.P.fname.pos); % load channel positions
load(DCM.Y.P.fname.g);   % load Green's function

xyzh = [DCM.xY.xyz];     % hemodynamic source positions
xyzd = R.d.xyzH;         % detector positions
ns = R.s.ns;             % number of sources (emitter)
nd = R.d.nd;             % number of detectors
nh = size(xyzh, 2);      % number of hemodynamic sources

nnode = size(G.xyz, 2);  % number of nodes
r = DCM.options.rs .* 3; % maximum distance between point and distributed sources
rois = DCM.Y.P.rois;

%--------------------------------------------------------------------------
%-Find indices of Green's function of detector and hemodynamic source
node_d = zeros(nd, 1);
for i = 1:nd
    diff = G.xyz - xyzd(:, i) * ones(1, nnode);
    diff = sum(diff.^2);
    node_d(i, 1) = find(diff == min(diff));
end

if isfield(G, 'elem')
    [indx] = find(G.elem(5,:) == 3); % grey matter
    elem = G.elem(1:4, indx);
    node_g = sort(unique(elem(:))); % indx_node_grey
else
    node_g = 1:size(G.xyz, 2);
end

ng = length(node_g);
node_h = [];
A = [];

for i = 1:nh
    diff = G.xyz(:, node_g) - xyzh(:,i) * ones(1, ng);
    diff = sqrt(sum(diff.^2));

    if r == 0,
        A.node_h(i, 1) = node_g(find(diff == min(diff)));
    else
        indx_r = find(diff < r);
        [dist_h, indx_h] = sort(diff(indx_r));
        node_h = node_g(indx_r(indx_h));

        A(i).dist_h = dist_h;
        A(i).node_h = node_h;
    end
end

% Calculate sensitivity matrix using Green's function
%--------------------------------------------------------------------------
nwav = size(G.s, 3); % number of wavelengths
nroi = length(rois);

for i = 1:nwav
    niter = size(A, 2);
    for j = 1:niter
        S = [];
        for k = 1:nroi,
            indx_s = find(R.s.label == R.ch_sd(rois(k), 2));
            indx_d = find(R.d.label == R.ch_sd(rois(k), 3));
            S(k,:) = (G.s(indx_s, A(j).node_h,i) .* G.d(indx_d, A(j).node_h,i))./(G.s(indx_s, node_d(indx_d),i));
        end
        A(j).(sprintf('S%d',i)) = S;
    end
end