function model = forestTrain(X, Y, opts)
    % X is NxD, where rows are data points
    % for convenience, for now we assume X is 0 mean unit variance. If it
    % isn't, preprocess your data with
    %
    % X= bsxfun(@rdivide, bsxfun(@minus, X, mean(X)), var(X) + 1e-10);
    %
    % If this condition isn't satisfied, some weak learners won't work out
    % of the box in current implementation.
    %
    % Y is discrete Nx1 vector of labels
    % model can be plugged into forestTest()
    %
    % decent default opts are:
    % opts.depth= 9;
    % opts.numTrees= 100; %(but more is _ALWAYS_ better, monotonically, no exceptions)
    % opts.numSplits= 5;
    % opts.classifierID= 2
    % opts.classifierCommitFirst= true;
    %
    % which means use depth 9 trees, train 100 of them, use 5 random
    % splits when training each weak learner. The last option controls
    % whether each node commits to a weak learner at random and then trains
    % it best it can, or whether each node tries all weak learners and
    % takes the one that does best. Empirically, it appears leaving this as
    % true (default) gives slightly better looking results.
    %

    numTrees= 100;
    verbose= false;

    if nargin < 3, opts= struct; end
    if isfield(opts, 'numTrees'), numTrees= opts.numTrees; end
    if isfield(opts, 'verbose'), verbose= opts.verbose; end

    treeModels= cell(1, numTrees);
    for i=1:numTrees

        treeModels{i} = treeTrain(X, Y, opts);

        % print info if verbose
        if verbose
            p10= floor(numTrees/10);
            if mod(i, p10)==0 || i==1 || i==numTrees
                fprintf('Training tree %d/%d...\n', i, numTrees);
            end
        end
    end

    model.treeModels = treeModels;
end


function mr = mrReadVAnat(pth)
% Read a mrGray vAnatomy .dat file into an mr struct.
%
% mr = mrReadVAnat([path to file])
%
% Loads the vAnatomy.dat file specified by fileName (full path!)
% into the [rows,cols,planes] image cube 'img'.
%
% If fileName is omitted, a get file dialog appears.
%
% RETURNS:
%   * img is the [rows,cols,planes] intensity array
%   * mmPerPix is the voxel size (in mm/pixel units)
%   * fileName is the full-path to the vAnatomy.dat file. (If
%     you pass fileName in, you obviously don't need this. But
%     it may be useful when the user selects the file.)
%
% ras, 06/30/05: imported into mrVista 2.0 Test repository.

% initialize mr struct
[p f ext] = fileparts(pth);
mr = mrCreateEmpty;
mr.format = 'vanat';
mr.pth = pth;
mr.name = f;

[mr.data, mr.voxelSize, mr.dims] = readFileVAnat(pth);
mr.data = double(mr.data);
mr.spaces = mrStandardSpaces(mr);

% based on the usual format of vAnatomy files, we can
% label the directions in the pixel space with a good guess:
mr.spaces(1).dirLabels = {'S <--> I'  'A <--> P'  'L <--> R'};
mr.spaces(1).sliceLabels =  {'Axial' 'Coronal' 'Sagittal'};
mr.spaces(2).dirLabels = {'S <--> I'  'A <--> P'  'L <--> R'};
mr.spaces(2).sliceLabels =  {'Axial' 'Coronal' 'Sagittal'};

mr.spaces(end+1) = mr.spaces(2); % add I|P|R space
mr.spaces(end).name = 'I|P|R';

mr.spaces(end+1) = mr.spaces(end);
mr.spaces(end).name = 'R|A|S';
mr.spaces(end).dirLabels =  {'L <--> R' 'P <--> A' 'I <--> S'};
mr.spaces(end).sliceLabels =  {'Sagittal' 'Coronal' 'Axial'};
mr.spaces(end).xform = ipr2ras(mr.spaces(4).xform,mr.dims,mr.voxelSize);

mr.dimUnits = {'mm' 'mm' 'mm' 'sec'};
mr.dataUnits = 'Scaled T1 Intensity';

return


function res = bf_output_image_powcorr(BF, S)
% Computes phase-amplitude coupling
% Copyright (C) 2012 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak using code bits from OSL library
% $Id: bf_output_image_powcorr.m 7703 2019-11-22 12:06:29Z guillaume $

%--------------------------------------------------------------------------
if nargin == 0
    all = cfg_const;
    all.tag = 'all';
    all.name = 'All';
    all.val  = {1};

    condlabel = cfg_entry;
    condlabel.tag = 'condlabel';
    condlabel.name = 'Condition label';
    condlabel.strtype = 's';
    condlabel.val = {''};

    conditions = cfg_repeat;
    conditions.tag = 'conditions';
    conditions.name = 'Conditions';
    conditions.help = {'Specify the labels of the conditions to be included in the inversion'};
    conditions.num  = [1 Inf];
    conditions.values  = {condlabel};
    conditions.val = {condlabel};

    whatconditions = cfg_choice;
    whatconditions.tag = 'whatconditions';
    whatconditions.name = 'What conditions to include?';
    whatconditions.values = {all, conditions};
    whatconditions.val = {all};

    sametrials = cfg_menu;
    sametrials.tag = 'sametrials';
    sametrials.name = 'Trials same as for filters';
    sametrials.labels = {'yes', 'no'};
    sametrials.values = {true, false};
    sametrials.val = {false};
    sametrials.help = {'Take the same trials as used for filter computation',...
        'This is useful for bootstrap.'};

    woi = cfg_entry;
    woi.tag = 'woi';
    woi.name = 'Time window of interest';
    woi.strtype = 'r';
    woi.num = [1 2];
    woi.val = {[-Inf Inf]};
    woi.help = {'Time windows (in ms)'};

    freqref         = cfg_entry;
    freqref.tag     = 'freqref';
    freqref.name    = 'Reference frequencies';
    freqref.strtype = 'r';
    freqref.num     = [1 Inf];
    freqref.val     = {20};
    freqref.help    = {'Frequencies in the reference channel'};

    resref         = cfg_entry;
    resref.tag     = 'resref';
    resref.name    = 'Reference resolutions';
    resref.strtype = 'r';
    resref.num     = [1 Inf];
    resref.val     = {5};
    resref.help    = {'Frequency resolution for reference frequencies. Single value or vector'};

    freq         = cfg_entry;
    freq.tag     = 'freq';
    freq.name    = 'Data frequencies';
    freq.strtype = 'r';
    freq.num     = [1 Inf];
    freq.val     = {20};
    freq.help    = {'First set of frequencies'};

    res         = cfg_entry;
    res.tag     = 'res';
    res.name    = 'Data resolutions';
    res.strtype = 'r';
    res.num     = [1 Inf];
    res.val     = {5};
    res.help    = {'Frequency resolution for data frequencies. Single value or vector'};

    refchan = cfg_entry;
    refchan.tag = 'refchan';
    refchan.name = 'Reference channel';
    refchan.strtype = 's';
    refchan.num = [1 Inf];
    refchan.help = {'Reference channel name.'};

    movavg         = cfg_entry;
    movavg.tag     = 'movavg';
    movavg.name    = 'Moving average window';
    movavg.strtype = 'r';
    movavg.num     = [1 1];
    movavg.val     = {100};
    movavg.help    = {'Time window for moving average of power envelope (ms).',...
        'Specify 0 to not average'};

    movavg         = cfg_entry;
    movavg.tag     = 'movavg';
    movavg.name    = 'Moving average window';
    movavg.strtype = 'r';
    movavg.num     = [1 1];
    movavg.val     = {100};
    movavg.help    = {'Time window for moving average of power envelope (ms).',...
        'Specify 0 to not average'};

    subsample = cfg_entry;
    subsample.tag = 'subsample';
    subsample.name = 'Subsample';
    subsample.strtype = 'n';
    subsample.num = [1 1];
    subsample.val = {1};
    subsample.help = {'Set to N to subsample the power to every Nth sample'};

    shuffle         = cfg_entry;
    shuffle.tag     = 'shuffle';
    shuffle.name    = 'Shuffle';
    shuffle.strtype = 'w';
    shuffle.num     = [1 1];
    shuffle.help    = {'Shuffle the reference channel to produce the null case.',...
        'Specify the number of shufflings'};
    shuffle.val = {0};

    modality         = cfg_menu;
    modality.tag     = 'modality';
    modality.name    = 'Modality';
    modality.help    = {'Specify modality'};
    modality.labels  = {
        'MEG'
        'MEGPLANAR'
        'EEG'
        }';
    modality.values  = {
        'MEG'
        'MEGPLANAR'
        'EEG'
        }';
    modality.val = {'MEG'};

    image_powcorr      = cfg_branch;
    image_powcorr.tag  = 'image_powcorr';
    image_powcorr.name = 'Power correlations image';
    image_powcorr.val  = {whatconditions, sametrials, shuffle, woi, refchan, freqref, ....
        resref, freq, res, movavg, subsample, modality};

    res = image_powcorr;

    return
elseif nargin < 2
    error('Two input arguments are required');
end

D = BF.data.D;

S.woi = 1e-3*S.woi; % ms -> s

samples =  D.indsample(S.woi(1)):D.indsample(S.woi(2));
nsamples = length(samples);
times = D.time(samples);

if isfield(S.whatconditions, 'all')
    S.whatconditions.condlabel = D.condlist;
end

for i = 1:numel(S.whatconditions.condlabel)
    if S.sametrials
        trials{i} = BF.features.trials(strmatch(S.whatconditions.condlabel{i},...
            D.conditions(BF.features.trials)));
    else
        trials{i} = D.indtrial(S.whatconditions.condlabel{i}, 'GOOD');
    end

    if isempty(trials{i})
        error('No trials matched the selection.');
    end

end

if isempty(trials)
    error('No trials matched the selection, check the specified condition labels');
end

channels = BF.features.(S.modality).chanind;
U        = BF.features.(S.modality).U;
nchan    = size(U, 2);

alltrials = spm_vec(trials);
ntrials   = length(alltrials);

nref = length(S.freqref);
nfreq    = length(S.freq);

W = BF.inverse.(S.modality).W;
nvert = numel(W);

Y = U'*reshape(D(channels, samples, alltrials), nchan, []);
Y = reshape(Y, size(Y, 1), nsamples, ntrials);

Yr = squeeze(D(D.indchannel(S.refchan), samples, alltrials));
if size(Yr, 1) == 1
    Yr = Yr';
end

spectrum = ft_specest_hilbert(Yr', times,...
    'freqoi', S.freqref, 'width', S.resref, 'filttype', 'but', 'filtorder', 2,...
    'filtdir', 'twopass', 'verbose', 0);

spectrum = reshape(permute(spectrum, [3 1 2]), nsamples, ntrials*nref);
spectrum = abs(spectrum);

if S.movavg
    avwin =  spm_hanning(1e-3*(S.movavg)*D.fsample);
    spectrum = conv2(avwin, 1, spectrum, 'same');
end

spectrum = spectrum(1:S.subsample:end, :);

spectrum = detrend(spectrum);
spectrum = spectrum./repmat(std(spectrum), size(spectrum, 1), 1);

refsig   = reshape(spectrum, [], ntrials, nref);

powc   = nan(length(S.freqref), length(S.freq), nvert);

if S.shuffle
    spowc = repmat(powc, [1 1 1 S.shuffle]);
    sind = zeros(S.shuffle, ntrials);
    for s = 1:S.shuffle
        sind(s, :) = randperm(ntrials);
    end
end

for f = 1:length(S.freq)

    spm_progress_bar('Init', ntrials, ...
        sprintf('Computing data spectra')); drawnow;
    if ntrials > 100, Ibar = floor(linspace(1, ntrials,100));
    else Ibar = 1:ntrials; end

    Yh = 0*Y;
    for i = 1:ntrials
        Yh(: , : ,i) = spm_squeeze(ft_specest_hilbert(squeeze(Y(:,:, i)), times,...
            'freqoi', S.freq(f), 'width', S.res(f), 'filttype', 'but', ...
            'filtorder', 2,  'filtdir', 'twopass', 'verbose', 0), 2);

        if ismember(i, Ibar)
            spm_progress_bar('Set', i); drawnow;
        end
    end

    Yh = reshape(Yh, nchan, []);

    spm_progress_bar('Clear');

    spm_progress_bar('Init', nvert, ...
        sprintf('Scanning grid points image')); drawnow;
    if nvert > 100, Ibar = floor(linspace(1, nvert,100));
    else Ibar = 1:nvert; end

    for i = 1:nvert
        if ~isnan(W{i})
            w    = W{i};

            sYh  = w*Yh;

            sYh  = reshape(abs(sYh), nsamples, ntrials);

            if S.movavg
                sYh = conv2(avwin, 1, sYh, 'same');
            end

            sYh = sYh(1:S.subsample:end, :);

            sYh = detrend(sYh);

            sYh = sYh./repmat(std(sYh), size(sYh, 1), 1);

            x = sYh(:);

            for j = 1:nref

                rYh = spm_squeeze(refsig(:,:, j), 3);

                for shuffle = 0:S.shuffle
                    if shuffle
                        rYh = rYh(:, sind(shuffle, :));
                    end

                    y = rYh(:);

                    pinvx = pinv(x);
                    pe  = pinvx*y;
                    r   = y-x*pe;
                    vr  = diag(r'*r/(size(y,1)-size(x,2)));
                    vrp = pinv(x'*x)*vr;
                    cs  = pe/sqrt(vrp);

                    if shuffle
                        spowc(j, f, i, shuffle) = cs;
                    else
                        powc(j, f, i) = cs;
                    end
                end
            end
        end

        if ismember(i, Ibar)
            spm_progress_bar('Set', i); drawnow;
        end
    end
end

spm_progress_bar('Clear');

if max(nfreq, nref)>1
    image(1).val     = squeeze(sum(sum(powc, 2), 1));
    image(1).label   = ['sumpowc_'  spm_file(D.fname, 'basename')];
    c = 2;
else
    c = 1;
end

for f = 1:nref
    for g = 1:nfreq
        image(c).val     = squeeze(powc(f, g, :));
        image(c).label   = ['powc_ref_' num2str(S.freqref(f)) 'Hz_meg_'...
            num2str(S.freq(g)) 'Hz_' spm_file(D.fname, 'basename')];
        c = c+1;
    end
end

for shuffle = 1:S.shuffle
    if max(nfreq, nref)>1
        image(c).val     = squeeze(sum(sum(spowc(:,:,:, shuffle), 2), 1));
        image(c).label   = ['shuffled' num2str(shuffle) '_sumpowc_'  spm_file(D.fname, 'basename')];
        c = c+1;
    end

    for f = 1:nref
        for g = 1:nfreq
            image(c).val     = squeeze(spowc(f, g, :, shuffle));
            image(c).label   = ['shuffled' num2str(shuffle) '_powc_ref_' num2str(S.freqref(f)) 'Hz_meg_'...
                num2str(S.freq(g)) 'Hz_' spm_file(D.fname, 'basename')];
            c = c+1;
        end
    end
end

res = image;


%Frame Coordinate frame object
%
% F = Frame(P, OPTIONS) creates an object that graphically renders
% a coordinate frame for SE(2), SO(2) or SE(3) represented by the
% pose P which can be:
% - homogeneous transform (3x3) for SE(2)
% - Quaternion for SO(3)
% - orthonormal rotation matrix (3x3) for SO(3)
% - homogeneous transform (4x4) for SE(3)
%
% Methods::
% move       move the graphical coordinate frame to a new pose
% animate    move the graphical coordinate frame to a new pose
% char
% display
% delete
%
% Options::
% 'color',C          The color to draw the axes, MATLAB colorspec C
% 'noaxes'           Don't display axes on the plot
% 'axis',A           Set dimensions of the MATLAB axes to A=[xmin xmax ymin ymax zmin zmax]
% 'frame',F          The frame is named {F} and the subscript on the axis labels is F.
% 'text_opts', opt   A cell array of MATLAB text properties
% 'handle',H         Draw in the MATLAB axes specified by the axis handle H
% 'view',V           Set plot view parameters V=[az el] angles, or 'auto'
%                    for view toward origin of coordinate frame
% 'arrow'            Use arrows rather than line segments for the axes
% 'width', w         Width of arrow tips
%
% Examples::
%
%       f_a = Frame(TA, 'frame', 'A')
%       f_b = Frame(TB, 'frame', 'B', 'color', 'b')
%       f_c = Frame(TC, 'frame', 'C', 'text_opts', {'FontSize', 10, 'FontWeight', 'bold'})
%
%       f_a.move(T);
%
% Notes::
% - The arrow option requires the third party package arrow3.
%
% See also TRPLOT2, TRANIMATE.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

% TODO
%  need to decide how to handle scaling
%  what does hold on mean?  don't touch scaling?

classdef Frame < handle

    properties
        T
        se2
        name
        hg
    end

    methods

        function f = Frame(T, varargin)

            if size(T,3) > 1
                error('trplot cannot operate on a sequence');
            end

            if ~ishomog(T) && ~isrot(T)
                error('trplot operates only on transform (4x4) or rotation matrix (3x3)');
            end

            opt.color = 'b';
            opt.axes = true;
            opt.axis = [];
            opt.frame = [];
            opt.text_opts = [];
            opt.view = [];
            opt.width = 1;
            opt.arrow = false;
            opt.handle = [];
            opt.se2 = false;

            opt = tb_optparse(opt, varargin);

            f.se2 = opt.se2;
            f.name = opt.frame;

            % axis labels
            if isempty(opt.frame)
                fmt = '%c';
            else
                fmt = sprintf('%%c_{%s}', opt.frame);
            end

            % text label options
            if isempty(opt.text_opts)
                opt.text_opts = {};
            end

            if isempty(opt.axis)
                % determine some default axis dimensions

                d = 1.2;
                if opt.se2
                    c = transl(T);
                    d = 1.2;
                    opt.axis = [c(1)-d c(1)+d c(2)-d c(2)+d];
                else
                    % get the origin of the frame
                    if isrot(T)
                        c = [0 0 0];  % at zero for a rotation matrix
                    else
                        c = transl(T);
                    end
                    opt.axis = [c(1)-d c(1)+d c(2)-d c(2)+d c(3)-d c(3)+d];
                end
            end

            % create the axes
            if ~isempty(opt.handle)
                hax = opt.handle;
                hold(hax);
            else
                ih = ishold;
                if ~ih
                    % if hold is not on, then clear the axes and set scaling
                    cla
                    if ~isempty(opt.axis)
                        axis(opt.axis);
                    end
                    daspect([1 1 1]);

                    if opt.axes
                        xlabel( 'X');
                        ylabel( 'Y');
                        zlabel( 'Z');
                        rotate3d on
                    end
                    new_plot = true;
                end
                hax = gca;
                hold on
            end

            opt.text_opts = {opt.text_opts{:}, 'Color', opt.color};

            % create the transfor for the frame, this allows the whole
            % graphical structure to be easily moved
            hg = hgtransform('Parent', hax);
            f.hg = hg;
            set(f.hg, 'Tag', 'Frame');
            set(f.hg, 'UserData', f);

            if opt.se2
                % create unit vectors
                o =  [0 0]';
                x1 = [1 0]';
                y1 = [0 1]';

                % draw the axes
                mstart = [o o]';
                mend = [x1 y1]';

                if opt.arrow
                    % draw the 2 arrows
                    S = [opt.color num2str(opt.width)];
                    ha = arrow3(mstart, mend, S);
                    for h=ha'
                        set(h, 'Parent', hg);
                    end
                else
                    for i=1:2
                        plot2([mstart(i,1:2); mend(i,1:2)], ...
                            'Color', opt.color, 'Parent', hg);
                    end
                end

                % add the labels to each axis
                h = text(x1(1), x1(2), sprintf(fmt, 'X'), 'Parent', hg);
                if ~isempty(opt.text_opts)
                    set(h, opt.text_opts{:});
                end

                h = text(y1(1), y1(2), sprintf(fmt, 'Y'), 'Parent', hg);
                if ~isempty(opt.text_opts)
                    set(h, opt.text_opts{:});
                end

                % label the frame
                if ~isempty(opt.frame)
                    h = text(o(1)-0.04*x1(1), o(2)-0.04*y1(2), ...
                        ['\{' opt.frame '\}'], 'Parent', hg);
                    set(h, 'VerticalAlignment', 'middle', ...
                        'HorizontalAlignment', 'center', opt.text_opts{:});
                end
            else
                % create unit vectors
                o =  [0 0 0]';
                x1 = [1 0 0]';
                y1 = [0 1 0]';
                z1 = [0 0 1]';

                % draw the axes
                mstart = [o o o]';
                mend = [x1 y1 z1]';

                if opt.arrow
                    % draw the 3 arrows
                    S = [opt.color num2str(opt.width)];
                    ha = arrow3(mstart, mend, S);
                    for h=ha'
                        set(h, 'Parent', hg);
                    end
                else
                    for i=1:3
                        h = plot2([mstart(i,1:3); mend(i,1:3)], ...
                            'Color', opt.color, 'Parent', hg);
                    end
                end

                % add the labels to each axis
                h = text(x1(1), x1(2), x1(3), sprintf(fmt, 'X'), 'Parent', hg);
                set(h, opt.text_opts{:});

                h = text(y1(1), y1(2), y1(3), sprintf(fmt, 'Y'), 'Parent', hg);
                set(h, opt.text_opts{:});

                h = text(z1(1), z1(2), z1(3), sprintf(fmt, 'Z'), 'Parent', hg);
                set(h, opt.text_opts{:});

                % label the frame
                if ~isempty(opt.frame)
                    h = text(o(1)-0.04*x1(1), o(2)-0.04*y1(2), o(3)-0.04*z1(3), ...
                        ['\{' opt.frame '\}'], 'Parent', hg);
                    set(h, 'VerticalAlignment', 'middle', ...
                        'HorizontalAlignment', 'center', opt.text_opts{:});
                end
            end

            if ~opt.axes
                set(gca, 'visible', 'off');
            end
            if isstr(opt.view) && strcmp(opt.view, 'auto')
                cam = x1+y1+z1;
                view(cam(1:3));
            elseif ~isempty(opt.view)
                view(opt.view);
            end

            if isempty(opt.handle) && ~ih
                grid on
                hold off
            end

            % now place the frame in the desired pose
            f.move(T);
        end

        function move(f, T)
            if f.se2
                if ~all(size(T) == [3 3])
                    error('expecting SE(2) matrix');
                end
                T = [T(1:2,1:2) zeros(2,1) T(1:2,3); 0 0 1 0; 0 0 0 1];
            elseif isrot(T)
                T = r2t(T);
            elseif ~ishomog(T)
                error('expecting SO(3) or SE(3) matrix');
            end

            % search for this named frame in all figs
            set(f.hg, 'Matrix', T);
            f.T = T;
        end

        function animate(f, P2, varargin)
        %ANIMATE Animate a coordinate frame
        %
        % ANIMATE(P1, P2, OPTIONS) animates a 3D coordinate frame moving from pose P1
        % to pose P2.  Poses P1 and P2 can be represented by:
        %   - homogeneous transformation matrices (4x4)
        %   - orthonormal rotation matrices (3x3)
        %   - Quaternion
        %
        % ANIMATE(P, OPTIONS) animates a coordinate frame moving from the identity pose
        % to the pose P represented by any of the types listed above.
        %
        % ANIMATE(PSEQ, OPTIONS) animates a trajectory, where PSEQ is any of
        %   - homogeneous transformation matrix sequence (4x4xN)
        %   - orthonormal rotation matrix sequence (3x3xN)
        %   - Quaternion vector (Nx1)
        %
        % Options::
        %  'fps', fps    Number of frames per second to display (default 10)
        %  'nsteps', n   The number of steps along the path (default 50)
        %  'axis',A      Axis bounds [xmin, xmax, ymin, ymax, zmin, zmax]
        %
        % See also TRPLOT.

            opt.fps = 10;
            opt.nsteps = 50;
            opt.axis = [];

            [opt, args] = tb_optparse(opt, varargin);

            P1 = [];

            % convert quaternion and rotation matrix to hom transform
            if isa(P2, 'Quaternion')
                T2 = P2.T;   % convert quaternion to transform
                if ~isempty(args) && isa(args{1},'Quaternion')
                    P1 = T2;
                    Q2 = args{1};
                    T2 = Q2.T;
                    args = args(2:end);
                else
                    T1 = eye(4,4);
                end
            elseif isrot(P2)
                T2 = r2t(P2);
                if ~isempty(args) && isrot(args{1})
                    P1 = T2;
                    T2 = r2t(args{1});
                    args = args(2:end);
                else
                    T1 = eye(4,4);
                end
            elseif ishomog(P2)
                T2 = P2;
                if ~isempty(args) && ishomog(args{1})
                    P1 = T2;
                    T2 = args{1};
                    args = args(2:end);
                else
                    T1 = eye(4,4);
                end
            end

            % at this point
            %   T1 is the initial pose
            %   T2 is the final pose
            %
            %  T2 may be a sequence

            if size(T2,3) > 1
                % tranimate(Ts)
                % we were passed a homog sequence
                if ~isempty(P1)
                    error('only 1 input argument if sequence specified');
                end
                Ttraj = T2;
            else
                % tranimate(P1, P2)
                % create a path between them
                Ttraj = ctraj(T1, T2, opt.nsteps);
            end

            if isempty(opt.axis)
                % create axis limits automatically based on motion of frame origin
                t = transl(Ttraj);
                mn = min(t) - 1.5;  % min value + length of axis + some
                mx = max(t) + 1.5;  % max value + length of axis + some
                axlim = [mn; mx];
                axlim = axlim(:)';
                args = [args 'axis' axlim];
            end

            % animate it for all poses in the sequence
            for i=1:size(Ttraj,3)
                T = Ttraj(:,:,i);
                f.move(T);
                pause(1/opt.fps);
            end
        end

        function delete(f)
        % DELETE Delete the coordinate frame
            children = get(f.hg, 'Children');
            for child=children
                delete(child);
            end
        end

        function s = char(f)
        %Link.char String representation of parameters
        %
        % s = L.char() is a string showing link parameters in compact single line format.
        % If L is a vector of Link objects return a string with one line per Link.
        %
        % See also Link.display.
            ts = trprint(f.T, 'rpy');
            s = sprintf(' {%s} %s', f.name, ts);
            if f.se2
                s = [s ' :: SE(2)'];
            else
                s = [s ' :: SE(3)'];
            end
        end

        function display(l)
        %Frame.display Display parameters
        %
        % F.display() display link parameters in compact single line format.  If L is a
        % vector of Link objects display one line per element.
        %
        % Notes::
        % - this method is invoked implicitly at the command line when the result
        %   of an expression is a Link object and the command has no trailing
        %   semicolon.
        %
        % See also Link.char, Link.dyn, SerialLink.showlink.
            loose = strcmp( get(0, 'FormatSpacing'), 'loose');
            if loose
                disp(' ');
            end
            disp([inputname(1), ' = '])
            disp( char(l) );
        end % display()

        function rescale(f)
            mn = [Inf Inf Inf];
            mx = -[Inf Inf Inf];
            for frame=findobj('Tag', 'Frame')'
                %%T = frame.

            end
        end

    end % methods
end


function [packet, state]= bbci_control_ERPSpeller(cfy_out, state, event, opt)
%BBCI_CONTROL_ERP_SPELLER - Generate control signal for ERP-based Hex-o-Spell
%
%Synopsis:
%  PACKET= bbci_control_ERPSpeller(CFY_OUT, EVENT, SETTINGS, STATE)
%
%Arguments:
%  CFY_OUT - Output of the classifier
%  STATE - Internal state variable
%  EVENT - Structure that specifies the event (fields 'time' and 'desc')
%      that triggered the evaluation of this control.
%  SETTINGS - Structure specifying the following feedback paramters:
%    .nClasses - number of classes of stimuli from which to select
%    .nSequences - number of sequences (repetitions) that should be used
%        in order to determine one choice
%
%Output:
% PACKET: Variable/value list in a CELL defining the control signal that
%     is to be sent via UDP to the Speller application.
% STATE: Updated internal state variable

% 02-2011 Benjamin Blankertz

if isempty(state),
  state.counter= zeros(1, opt.nClasses);
  state.output= zeros(1, opt.nClasses);
end

this_cue= opt.mrk2feedback_fcn(event.desc);
state.counter(this_cue)= state.counter(this_cue) + 1;
state.output(this_cue)= state.output(this_cue) + cfy_out;

if sum(state.counter) >= opt.nClasses*opt.nSequences,
  idx= find(state.counter>0);  % avoid divide by zero
  state.output(idx)= state.output(idx) ./ state.counter(idx);
  [max_score, selected_class]= min(state.output);
  packet= {'i:cl_output', selected_class};
  state.counter(:)= 0;
  state.output(:)= 0;
else
  packet= [];
end


function rtk=udrcvbias(rtk,tt)

global glc
VAR_HWBIAS=1^2; PRN_HWBIAS=1e-6;

for i=1:glc.NFREQGLO

    j=rtk.il+i;

    if rtk.x(j)==0
        rtk=initx(rtk,1e-6,VAR_HWBIAS,j);
    elseif rtk.nfix>=rtk.opt.minfix&&rtk.sol.ratio>rtk.opt.thresar(1)
        rtk=initx(rtk,rtk.xa(j),rtk.Pa(j,j),j);
    else
        rtk.P(j,j)=rtk.P(j,j)+PRN_HWBIAS^2*abs(tt);
    end

end

return


function printx_ppp(x,rtk)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2020-2025, by Kai Chen, All rights reserved.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
opt=rtk.opt; nf=rtk.NF;

fprintf('pos   = ');fprintf('%14.5f %14.5f %14.5f',x(1),x(2),x(3));fprintf('\n');

if rtk.NP==9
    fprintf('vel   = ');fprintf('%9.5f%9.5f%9.5f',x(4),x(5),x(6));fprintf('\n');
    fprintf('acc   = ');fprintf('%9.5f%9.5f%9.5f',x(7),x(8),x(9));fprintf('\n');
end

fprintf('clk   = ');
for i=1:glc.NSYS
    fprintf('%9.5f',x(rtk.ic+i));
end
fprintf('\n');

if rtk.NT>0
    fprintf('trop  = ');
    if opt.tropopt==glc.TROPOPT_EST
        fprintf('%9.5f',x(rtk.it+1));
    else
        fprintf('%9.5f',x(rtk.it+2));
        fprintf('%9.5f',x(rtk.it+3));
    end
    fprintf('\n');
end

if rtk.NI>0
    fprintf('iono  = ');
    for i=1:glc.MAXSAT
        if abs(x(rtk.ii+i))>=1e-6,fprintf('%9.5f',x(rtk.ii+i));end
    end
    fprintf('\n');
end

if rtk.ND>0
    fprintf('DCB   = ');
    fprintf('%9.5f',x(rtk.id+1));
    fprintf('\n');
end

if rtk.NB>0
    fprintf('bias1 = ');
    for i=1:glc.MAXSAT
        if abs(x(rtk.ib+i))>=1e-6,fprintf('%10.5f',x(rtk.ib+i));end
    end
    fprintf('\n');
    if nf>=2
        fprintf('bias2 = ');
        for i=1:glc.MAXSAT
            if abs(x(rtk.ib+glc.MAXSAT+i))>=1e-6
                fprintf('%10.5f',x(rtk.ib+glc.MAXSAT+i));
            end
        end
        fprintf('\n');
    end
    if nf>=3
        fprintf('bias3 = ');
        for i=1:glc.MAXSAT
            if abs(x(rtk.ib+glc.MAXSAT*2+i))>=1e-6
                fprintf('%10.5f',x(rtk.ib+glc.MAXSAT*2+i));
            end
        end
        fprintf('\n');
    end
end

return


filename='Throne_Tetrahedra_SYM';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'holes';
cost = {'compliance'};
weights = [1];
constraint = {'volume'};
optimizer = 'HAMILTON-JACOBI'; incrementFactor = 1;
filterType = 'P1';
constraint_case = 'INEQUALITY';
line_search_initiator = 'INCREASING LAST STEP';

nsteps = 10;
Vfrac_final = 0.1;
Perimeter_target=3.5;
optimality_final =1e-3;
constr_final =1e-3;

BCscale_factor = 0.3;
rotation_per_it = 20;
HJiter0 = 1;
e2 = 1;
N_holes = [6 3 3];
R_holes = 0.4;
phase_holes = [0 0 0];

Vfrac_initial = 0.7;
optimality_initial = 5e-2;
constr_initial = 5e-2;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;

% maxiter = 1;
TOL.nu_plus = 0.3;
TOL.nu_minus = 0.3;


function [obs,nobs]=matchobs(rtk,imud,obss)

global gls
imu_sample_rate=rtk.opt.ins.sample_rate;
time0=obss.data(:,1)+obss.data(:,2);

time1=imud.time.time+imud.time.sec;

idx=abs(time1-time0)<(0.501/imu_sample_rate);

if any(idx)
    obs0=obss.data(idx,:);  nobs=size(obs0,1);
else
    obs=NaN; nobs=0; return;
end

obs_tmp=repmat(gls.obs_tmp,nobs,1);
for i=1:nobs
    obs_tmp(i).time.time=obs0(i,1);
    obs_tmp(i).time.sec=obs0(i,2);
    obs_tmp(i).sat=obs0(i,3);
    obs_tmp(i).P=obs0(i,4:6);
    obs_tmp(i).L=obs0(i,7:9);
    obs_tmp(i).D=obs0(i,10:12);
    obs_tmp(i).S=obs0(i,13:15);
    obs_tmp(i).LLI=obs0(i,16:18);
    obs_tmp(i).code=obs0(i,19:21);
end

[obs,nobs]=exclude_sat(obs_tmp,rtk);

return


function []=ps_select(reest_flag,plot_flag)
% PS_SELECT select PS based on gamma and D_A
%   PS_SELECT(REEST_FLAG,PLOT_FLAG)
%   REEST_FLAG = 1 skip reestimation completely (default 0)
%   REEST_FLAG = 2 reuse previously reestimated gamma (default 0)
%   REEST_FLAG = 3 reestimate gamma for all candidate pixels (default 0)
%   PLOT_FLAG = 1 for plots (default 0)
%
%   Andy Hooper, June 2006
%
%   ======================================================================
%   07/2006 AH: Use specific bperp for re-estimation of gamma
%   08/2006 AH: Load workspaces into structures
%   09/2006 AH: Check if no gamma threshold meets criteria
%   09/2006 AH: add small baselines
%   09/2009 AH: add option to select on density instead of percentage
%   01/2010 KS: fixed badly conditioned polyfit errors by scaling and
%               centering
%   02/2010 AH: Only bin by D_A if enough candidate pixels
%   02/2010 AH: Leave out ifgs in drop_ifg_index from noise calculation
%   09/2010 JJS+MA: To make use oversampling dataset
%   12/2010 AH: Fix error message for density selection
%   02/2011 DB: Fix error with keep_ix
%   05/2012 AH: subtract only pixel being tested, not zero whole grid cell
%   01/2013 AH: Set default threshold if not enough random pixels
%   04/2015 DB: Give a warning to remove patch from patch list when no PS are left
%   09/2015 DB: Store information on nr of PS left. Support auto procesing
%   01/2016 DB: Replace save with stamps_save
%   02/2016 DB: Identified bug when patch size is smaller than filter size.
%               For now drop this patch. This needs to be fixed better.
%   08/2017 AH: Ensure coh_thresh not negative.
%   ======================================================================
logit;
logit('Selecting stable-phase pixels...')

if nargin<1
    reest_flag=0;
end

if nargin<2
    plot_flag=0;
end

psver=getpsver;
if psver>1
   setpsver(1)
end

slc_osf=getparm('slc_osf',1);  % [MA]
clap_alpha=getparm('clap_alpha',1);
clap_beta=getparm('clap_beta',1);
n_win=getparm('clap_win',1);
select_method=getparm('select_method',1);
if strcmpi(select_method,'PERCENT')
    max_percent_rand=getparm('percent_rand',1);
else
    max_density_rand=getparm('density_rand',1);
end
gamma_stdev_reject=getparm('gamma_stdev_reject',1);
small_baseline_flag=getparm('small_baseline_flag',1);
drop_ifg_index=getparm('drop_ifg_index',1);

if strcmpi(small_baseline_flag,'y')
    low_coh_thresh=15; % equivalent to coh of 15/100
else
    low_coh_thresh=31; % equivalent to coh of 31/100
end

load psver

psname=['ps',num2str(psver)];
phname=['ph',num2str(psver)];
pmname=['pm',num2str(psver)];
selectname=['select',num2str(psver)];
daname=['da',num2str(psver)];
bpname=['bp',num2str(psver)];

ps=load(psname);

ifg_index=setdiff([1:ps.n_ifg],drop_ifg_index);

if exist([phname,'.mat'],'file')
    phin=load(phname);
    ph=phin.ph;
    clear phin
else
    ph=ps.ph;
end

bperp=ps.bperp;
n_ifg=ps.n_ifg;
if ~strcmpi(small_baseline_flag,'y')
    master_ix=sum(ps.master_day>ps.day)+1;
    no_master_ix=setdiff([1:ps.n_ifg],ps.master_ix);
    ifg_index=setdiff(ifg_index,ps.master_ix);
    ifg_index(ifg_index>master_ix)=ifg_index(ifg_index>master_ix)-1;
    ph=ph(:,no_master_ix);
    bperp=bperp(no_master_ix);
    n_ifg=length(no_master_ix);
end
n_ps=ps.n_ps;
xy=ps.xy;

pm=load(pmname);
if exist([daname,'.mat'],'file')
    da=load(daname);
    D_A=da.D_A;
    clear da
else
    D_A=[];
end

if ~isempty(D_A) & size(D_A,1)>=10000
    % chunk up PSC
    D_A_sort=sort(D_A);
    if size(D_A,1)>=50000
      bin_size=10000;
    else
      bin_size=2000;
    end
    D_A_max=[0;D_A_sort(bin_size:bin_size:end-bin_size);D_A_sort(end)];

else
    D_A_max=[0;1];
    D_A=ones(size(pm.coh_ps));
end

if ~strcmpi(select_method,'PERCENT')
    patch_area=prod(max(xy(:,2:3),[],1)-min(xy(:,2:3),[],1))/1e6; % in km
    max_percent_rand=max_density_rand*patch_area/(length(D_A_max)-1);
end

min_coh=zeros(length(D_A_max)-1,1);
D_A_mean=zeros(size(D_A_max,1)-1,1);
Nr_dist=pm.Nr;

if reest_flag==3 % reestimate for all candidate pixels
    coh_thresh=0;
    coh_thresh_coeffs=[];
else
  for i=1:length(D_A_max)-1
    coh_chunk=pm.coh_ps(D_A>D_A_max(i) & D_A<=D_A_max(i+1));
    D_A_mean(i)=mean(D_A(D_A>D_A_max(i) & D_A<=D_A_max(i+1)));
    coh_chunk=coh_chunk(coh_chunk~=0);  % discard PSC for which coherence was not calculated
    Na=hist(coh_chunk,pm.coh_bins);
    Nr=Nr_dist*sum(Na(1:low_coh_thresh))/sum(Nr_dist(1:low_coh_thresh));
    if i==length(D_A_max)-1 & plot_flag==1
        figure
        plot(pm.coh_bins,Na,'g')
        hold on
        plot(pm.coh_bins,Nr,'r')
        legend('data','random')
        title('Before Gamma Reestimation')
    end
    Na(Na==0)=1; % avoid divide by zero
    if strcmpi(select_method,'PERCENT')
        percent_rand=fliplr(cumsum(fliplr(Nr))./cumsum(fliplr(Na))*100);
    else
        percent_rand=fliplr(cumsum(fliplr(Nr))); % absolute number
    end
    ok_ix=find(percent_rand<max_percent_rand);
    if isempty(ok_ix)
        min_coh(i)=1; % no threshold meets criteria
    else
        min_fit_ix=min(ok_ix)-3;
        if min_fit_ix<=0
            %min_coh(i)=low_coh_thresh/100;
            min_coh(i)=nan;
        else
            max_fit_ix=min(ok_ix)+2;
            max_fit_ix(max_fit_ix>100)=100; % make sure not higher than length of percent_rand
            [p,S,mu]=polyfit(percent_rand(min_fit_ix:max_fit_ix),[min_fit_ix*0.01:0.01:max_fit_ix*0.01],3); % KS
            min_coh(i)=polyval(p,max_percent_rand,[],mu); % KS
%             p=polyfit(percent_rand(min_fit_ix:max_fit_ix),[min_fit_ix*0.01:0.01:max_fit_ix*0.01],3);
%             min_coh(i)=polyval(p,max_percent_rand);
        end
    end
  end

  nonnanix=~isnan(min_coh);
  if sum(nonnanix)<1
      warning('Not enough random phase pixels to set gamma threshold - using default threshold of 0.3')
      coh_thresh=0.3;
      coh_thresh_coeffs=[];
  else
  min_coh=min_coh(nonnanix);
  D_A_mean=D_A_mean(nonnanix);
  if size(min_coh,1)>1
    coh_thresh_coeffs=polyfit(D_A_mean,min_coh,1);  % fit polynomial to the curve
    if coh_thresh_coeffs(1)>0 % positive slope (as expected)
        coh_thresh=polyval(coh_thresh_coeffs,D_A);
    else % unable to ascertain correct slope
        coh_thresh=polyval(coh_thresh_coeffs,0.35); % set an average threshold for all D_A
        coh_thresh_coeffs=[];
    end
  else
    coh_thresh=min_coh;
    coh_thresh_coeffs=[];
  end
 end
end

coh_thresh(coh_thresh<0)=0; % to ensures pixels with coh=0 are rejected

logit(sprintf('Initial gamma threshold: %.3f at D_A=%.2f to %.3f at D_A=%.2f',min(coh_thresh),min(D_A),max(coh_thresh),max(D_A)))

if  plot_flag==1
    figure
    plot(D_A_mean,min_coh,'*')
    hold on
    if ~isempty(coh_thresh_coeffs)
        plot(D_A_mean,polyval(coh_thresh_coeffs,D_A_mean),'r')
    end
    ylabel('\gamma_{thresh}')
    xlabel('D_A')
end

ix=find(pm.coh_ps>coh_thresh); % select those below threshold
n_ps=length(ix);
logit(sprintf('%d PS selected initially',n_ps))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% reject part-time PS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if gamma_stdev_reject>0
    ph_res_cpx=exp(j*pm.ph_res(:,ifg_index));
    coh_std=zeros(length(ix),1);
    for i=1:length(ix)
        coh_std(i)=std(bootstrp(100,@(ph) abs(sum(ph))/length(ph), ph_res_cpx(ix(i),ifg_index)));
    end
    clear ph_res_cpx
    ix=ix(coh_std<gamma_stdev_reject);
    n_ps=length(ix);
    logit(sfprintf('%d PS left after pps rejection',n_ps))
end

if reest_flag~=1

  if reest_flag~=2 % reestimate coh with the PS removed from filtered patch
    for i=1:length(drop_ifg_index);
      if strcmpi(small_baseline_flag,'y')
        logit(sprintf('%s-%s is dropped from noise re-estimation',datestr(ps.ifgday(drop_ifg_index(i),1)),datestr(ps.ifgday(drop_ifg_index(i),2))))
      else
        logit(sprintf('%s is dropped from noise re-estimation',datestr(ps.day(drop_ifg_index(i)))))
      end
    end
    pm=rmfield(pm,{'ph_res'});
    pm=rmfield(pm,{'ph_patch'});
    ph_patch2=zeros(n_ps,n_ifg,'single');
    ph_res2=zeros(n_ps,n_ifg,'single');
    ph=ph(ix,:);

    if length(coh_thresh)>1
        coh_thresh=coh_thresh(ix);
    end

    n_i=max(pm.grid_ij(:,1));
    n_j=max(pm.grid_ij(:,2));
    K_ps2=zeros(n_ps,1);
    C_ps2=zeros(n_ps,1);
    coh_ps2=zeros(n_ps,1);
    ph_filt=zeros(n_win,n_win,n_ifg);

    for i=1:n_ps
        ps_ij=pm.grid_ij(ix(i),:);
        i_min=max(ps_ij(1)-n_win/2,1);
        i_max=i_min+n_win-1;
        if i_max>n_i
            i_min=i_min-i_max+n_i;
            i_max=n_i;
        end
        j_min=max(ps_ij(2)-n_win/2,1);
        j_max=j_min+n_win-1;
        if j_max>n_j
            j_min=j_min-j_max+n_j;
            j_max=n_j;
        end

        % it could occur that your patch size is smaller than the filter size
        % crude bug fix is to drop this patch. It needs fixing in future...
        if j_min<1 || i_min<1
            % THIS NEEDS TO BECOME AN ACTUAL FIX, but not sure how...
            ph_patch2(i,:) =0;
        else

            % remove the pixel for which the smoothign is computed
            ps_bit_i=ps_ij(1)-i_min+1;
            ps_bit_j=ps_ij(2)-j_min+1;
            ph_bit=pm.ph_grid(i_min:i_max,j_min:j_max,:);
            ph_bit(ps_bit_i,ps_bit_j,:)=0;

            %ph_bit(ps_bit_i,ps_bit_j,:)=ph_bit(ps_bit_i,ps_bit_j,:)-shiftdim(pm.ph_weight(i,:),-1);

            % JJS oversample update for PS removal + [MA] general usage update
            ix_i=ps_bit_i-(slc_osf-1):ps_bit_i+(slc_osf-1);
            ix_i=ix_i(ix_i>0&ix_i<=size(ph_bit,1));
            ix_j=ps_bit_j-(slc_osf-1):ps_bit_j+(slc_osf-1);
            ix_j=ix_j(ix_j>0&ix_j<=size(ph_bit,2));
            ph_bit(ix_i,ix_j)=0;

            for i_ifg=1:n_ifg
                ph_filt(:,:,i_ifg)=clap_filt_patch(ph_bit(:,:,i_ifg),clap_alpha,clap_beta,pm.low_pass);
            end

            ph_patch2(i,:)=squeeze(ph_filt(ps_bit_i,ps_bit_j,:));
        end
        if i/10000==floor(i/10000)
            logit(sprintf('%d patches re-estimated',i))
        end

    end

    pm=rmfield(pm,{'ph_grid'});
    bp=load(bpname);
    bperp_mat=bp.bperp_mat(ix,:);
    clear bp

    for i=1:n_ps
        psdph=ph(i,:).*conj(ph_patch2(i,:));
        if sum(psdph==0)==0  % insist on a non-null value in every ifg
            psdph=psdph./abs(psdph);
            [Kopt,Copt,cohopt,ph_residual]=ps_topofit(psdph(ifg_index),bperp_mat(i,ifg_index)',pm.n_trial_wraps,'n');
            K_ps2(i)=Kopt(1);
            C_ps2(i)=Copt(1);
            coh_ps2(i)=cohopt(1);
            ph_res2(i,ifg_index)=angle(ph_residual);
        else
            K_ps2(i)=nan;
            coh_ps2(i)=nan;
        end
        if i/10000==floor(i/10000)
            logit(sprintf('%d coherences re-estimated',i))
        end
    end
  else % reest_flag==2, use previously recalculated coh
    sl=load(selectname);
    ix=sl.ix;
    coh_ps2=sl.coh_ps2;
    K_ps2=sl.K_ps2;
    C_ps2=sl.C_ps2;
    ph_res2=sl.ph_res2;
    ph_patch2=sl.ph_patch2;
  end
    pm.coh_ps(ix)=coh_ps2;

    % calculate threshold again based on recalculated coh
    for i=1:length(D_A_max)-1
        coh_chunk=pm.coh_ps(D_A>D_A_max(i) & D_A<=D_A_max(i+1));
        D_A_mean(i)=mean(D_A(D_A>D_A_max(i) & D_A<=D_A_max(i+1)));
        coh_chunk=coh_chunk(coh_chunk~=0);  % discard PSC for which coherence was not calculated
        Na=hist(coh_chunk,pm.coh_bins);
        Nr=Nr_dist*sum(Na(1:low_coh_thresh))/sum(Nr_dist(1:low_coh_thresh));
        if i==length(D_A_max)-1 & plot_flag==1
            figure
            plot(pm.coh_bins,Na,'g')
            hold on
            plot(pm.coh_bins,Nr,'r')
            legend('data','random')
            title('After Gamma Reestimation')
        end
        Na(Na==0)=1; % avoid divide by zero
        if strcmpi(select_method,'PERCENT')
            percent_rand=fliplr(cumsum(fliplr(Nr))./cumsum(fliplr(Na))*100);
        else
            percent_rand=fliplr(cumsum(fliplr(Nr))); % despite the name, percent_rand here is the absolute number
        end
        ok_ix=find(percent_rand<max_percent_rand);
        if isempty(ok_ix)
            min_coh(i)=1;
        else
            min_fit_ix=min(ok_ix)-3;
            if min_fit_ix<=0
                %min_coh(i)=low_coh_thresh/100;
                min_coh(i)=nan;
            else
                max_fit_ix=min(ok_ix)+2;
                max_fit_ix(max_fit_ix>100)=100; % make sure not higher than length of percent_rand
                [p,S,mu]=polyfit(percent_rand(min_fit_ix:max_fit_ix),[min_fit_ix*0.01:0.01:max_fit_ix*0.01],3); % KS
                min_coh(i)=polyval(p,max_percent_rand,[],mu); % KS
%                 p=polyfit(percent_rand(min_fit_ix:max_fit_ix),[min_fit_ix*0.01:0.01:max_fit_ix*0.01],3);
%                 min_coh(i)=polyval(p,max_percent_rand);
            end
        end
    end

    nonnanix=~isnan(min_coh);
    if sum(nonnanix)<1
        coh_thresh=0.3;
        coh_thresh_coeffs=[];
    else
        min_coh=min_coh(nonnanix);
        D_A_mean=D_A_mean(nonnanix);
        if length(min_coh)>1
        %keyboard
            coh_thresh_coeffs=polyfit(D_A_mean,min_coh,1);  % fit polynomial to the curve
            if coh_thresh_coeffs(1)>0 % positive slope (as expected)
                coh_thresh=polyval(coh_thresh_coeffs,D_A(ix));
            else % unable to ascertain correct slope
                coh_thresh=polyval(coh_thresh_coeffs,0.35); % set an average threshold for all D_A
                coh_thresh_coeffs=[];
            end
        else
            coh_thresh=min_coh;
            coh_thresh_coeffs=[];
        end
    end

    coh_thresh(coh_thresh<0)=0; % to ensures pixels with coh=0 are rejected

    logit(sprintf('Reestimation gamma threshold: %.3f at D_A=%.2f to %.3f at D_A=%.2f',min(coh_thresh),min(D_A),max(coh_thresh),max(D_A)))

    bperp_range=max(bperp)-min(bperp);
    keep_ix=coh_ps2>coh_thresh & abs(pm.K_ps(ix)-K_ps2)<2*pi/bperp_range;
    clear pm
    logit(sprintf('%d ps selected after re-estimation of coherence',sum(keep_ix)))
    clear pm

else % reest_flag==1, skip re-estimation
    pm=rmfield(pm,{'ph_grid'});
    ph_patch2=pm.ph_patch(ix,:);
    ph_res2=pm.ph_res(ix,:);
    K_ps2=pm.K_ps(ix);
    C_ps2=pm.C_ps(ix);
    coh_ps2=pm.coh_ps(ix);
    keep_ix=true(size(ix));
end % end-if reest_flag

%%% Keep information about number of PS left.
if exist('no_ps_info.mat','file')~=2
   stamps_step_no_ps = zeros([5 1 ]);       % keep for the first 5 steps only
else
   load('no_ps_info.mat');
   % reset as we are currently re-processing
   stamps_step_no_ps(3:end)=0;
end
if sum(keep_ix)==0
   fprintf('***No PS points left. Updating the stamps log for this****\n')
   % update the flag indicating no PS left in step 3
   stamps_step_no_ps(3)=1;
end
save('no_ps_info.mat','stamps_step_no_ps')

if  plot_flag==1
    figure
    plot(D_A_mean,min_coh,'*')
    hold on
    if ~isempty(coh_thresh_coeffs)
        plot(D_A_mean,polyval(coh_thresh_coeffs,D_A_mean),'r')
    end
    ylabel('\gamma_{thresh}')
    xlabel('D_A')
end

% save(selectname,'ix','keep_ix','ph_patch2','ph_res2','K_ps2','C_ps2','coh_ps2','coh_thresh','coh_thresh_coeffs','clap_alpha','clap_beta','n_win','max_percent_rand','gamma_stdev_reject','small_baseline_flag','ifg_index');
stamps_save(selectname,ix,keep_ix,ph_patch2,ph_res2,K_ps2,C_ps2,coh_ps2,coh_thresh,coh_thresh_coeffs,clap_alpha,clap_beta,n_win,max_percent_rand,gamma_stdev_reject,small_baseline_flag,ifg_index);

logit(1);


%
% Original source
% http://www.mat.univie.ac.at/~neum/glopt/coconut/Benchmark/Library1/alkyl.mod
%
function test_alkyl

  addpath('../lib') ;

  auxdata = {} ;
  options.lb = [ 0, 0,   0,   0, 0, 0.85, 0.9,  3,  1.2, 1.45, 0.99, 0.99, 0.9, 0.99 ] ;
  options.ub = [ 2, 1.6, 1.2, 5, 2, 0.93, 0.95, 12, 4,   1.62, 1.01010101010101, 1.01010101010101, 1.11111111111111, 1.01010101010101 ] ;

  % The constraint functions are bounded to zero
  options.cl = zeros(1,7); %  constraints
  options.cu = zeros(1,7);

  % Set up the auxiliary data.
  options.auxdata = auxdata ;

  % Set the IPOPT options.
  options.ipopt.jac_d_constant   = 'no';
  options.ipopt.hessian_constant = 'no';
  options.ipopt.mu_strategy      = 'adaptive';
  options.ipopt.max_iter         = 400;
  options.ipopt.tol              = 1e-10;
  options.ipopt.linear_solver    = 'ma57';
  %options.ipopt.linear_solver    = 'mumps';
  %options.ipopt.linear_solver    = 'pardiso';

  % The callback functions.
  funcs.objective         = @objective;
  funcs.constraints       = @constraints;
  funcs.gradient          = @gradient;
  funcs.jacobian          = @jacobian;
  funcs.jacobianstructure = @jacobianstructure;
  if true
    funcs.hessian           = @hessian;
    funcs.hessianstructure  = @hessianstructure;
    options.ipopt.derivative_test = 'second-order';
  else
    options.ipopt.hessian_approximation      = 'limited-memory';
    options.ipopt.limited_memory_update_type = 'bfgs' ; % {bfgs}, sr1 = 6; % {6}
    options.ipopt.derivative_test            = 'first-order';
  end

  % Run IPOPT.
  x0 = [ 1.745,   ...
         1.2,     ...
         1.1,     ...
         3.048,   ...
         1.974,   ...
         0.893,   ...
         0.928,   ...
         8,       ...
         3.6,     ...
         1.45,    ...
         1,       ...
         1,       ...
         1,       ...
         1 ] ;

  tic
  [x, info] = ipopt_auxdata(x0,funcs,options);
  elapsed = toc ;

  info;

  x

end

%%
% map the indices with the corresponding index in the spase matrix
function f = objective( x, auxdata )
  x2  = x(1) ;
  x3  = x(2) ;
  x4  = x(3) ;
  x5  = x(4) ;
  x6  = x(5) ;
  x7  = x(6) ;
  x8  = x(7) ;
  x9  = x(8) ;
  x10 = x(9) ;
  x11 = x(10) ;
  x12 = x(11) ;
  x13 = x(12) ;
  x14 = x(13) ;
  x15 = x(14) ;
  f   = - 6.3*x5*x8 + 5.04*x2 + 0.35*x3 + x4 + 3.36*x6 ;
end

%%
% map the indices with the corresponding index in the spase matrix
function g = gradient(x,auxdata)
  x2  = x(1) ;
  x3  = x(2) ;
  x4  = x(3) ;
  x5  = x(4) ;
  x6  = x(5) ;
  x7  = x(6) ;
  x8  = x(7) ;
  x9  = x(8) ;
  x10 = x(9) ;
  x11 = x(10) ;
  x12 = x(11) ;
  x13 = x(12) ;
  x14 = x(13) ;
  x15 = x(14) ;
  g   = [ 5.04, 0.35, 1, -6.3*x8, 3.36, 0, -6.3*x5, 0, 0, 0, 0, 0, 0, 0 ] ;
end

function c = constraints(x,auxdata)
  x2  = x(1) ;
  x3  = x(2) ;
  x4  = x(3) ;
  x5  = x(4) ;
  x6  = x(5) ;
  x7  = x(6) ;
  x8  = x(7) ;
  x9  = x(8) ;
  x10 = x(9) ;
  x11 = x(10) ;
  x12 = x(11) ;
  x13 = x(12) ;
  x14 = x(13) ;
  x15 = x(14) ;
  e2  =  - 0.819672131147541*x2 + x5 - 0.819672131147541*x6 ;
  e3  = 0.98*x4 - x7*(0.01*x5*x10 + x4) ;
  e4  =  - x2*x9 + 10*x3 + x6 ;
  e5  = x5*x12 - x2*(1.12 + 0.13167*x9 - 0.0067*x9*x9) ;
  e6  = x8*x13 - 0.01*(1.098*x9 - 0.038*x9*x9) - 0.325*x7 - 0.57425;
  e7  = x10*x14 + 22.2*x11 - 35.82;
  e8  = x11*x15 - 3*x8 + 1.33 ;
  c   = [e2;e3;e4;e5;e6;e7;e8] ;
end

function jac = jacobian(x,auxdata)
  x2  = x(1) ;
  x3  = x(2) ;
  x4  = x(3) ;
  x5  = x(4) ;
  x6  = x(5) ;
  x7  = x(6) ;
  x8  = x(7) ;
  x9  = x(8) ;
  x10 = x(9) ;
  x11 = x(10) ;
  x12 = x(11) ;
  x13 = x(12) ;
  x14 = x(13) ;
  x15 = x(14) ;

  jac = sparse(7,14) ;

  jac(1,1) = -0.819672131147541 ;
  jac(1,4) = 1 ;
  jac(1,5) = -0.819672131147541 ;

  jac(2,3) = 0.98 - x7 ;
  jac(2,4) = -x7*(0.01*x10);
  jac(2,6) = -(0.01*x5*x10 + x4);
  jac(2,9) = - x7*(0.01*x5);

  jac(3,1) = -x9 ;
  jac(3,2) = 10 ;
  jac(3,5) = 1 ;
  jac(3,8) = -x2 ;

  jac(4,1)  = -(1.12 + 0.13167*x9 - 0.0067*x9*x9) ;
  jac(4,4)  = x12 ;
  jac(4,8)  = -x2*(0.13167 - 2*0.0067*x9) ;
  jac(4,11) = x5 ;

  jac(5,6)  = -0.325;
  jac(5,7)  = x13 ;
  jac(5,8)  = - 0.01*(1.098 - 2*0.038*x9) ;
  jac(5,12) = x8 ;

  jac(6,9)  = x14;
  jac(6,10) = 22.2;
  jac(6,13) = x10;

  jac(7,7)  = -3 ;
  jac(7,10) = x15 ;
  jac(7,14) = x11 ;

end

function jac = jacobianstructure(auxdata)
  jac = sparse(7,14) ;

  jac(1,1) = 1 ;
  jac(1,4) = 1 ;
  jac(1,5) = 1 ;

  jac(2,3) = 1 ;
  jac(2,4) = 1 ;
  jac(2,6) = 1 ;
  jac(2,9) = 1 ;

  jac(3,1) = 1 ;
  jac(3,2) = 1 ;
  jac(3,5) = 1 ;
  jac(3,8) = 1 ;

  jac(4,1)  = 1 ;
  jac(4,4)  = 1 ;
  jac(4,8)  = 1 ;
  jac(4,11) = 1 ;

  jac(5,6)  = 1 ;
  jac(5,7)  = 1 ;
  jac(5,8)  = 1 ;
  jac(5,12) = 1 ;

  jac(6,9)  = 1 ;
  jac(6,10) = 1 ;
  jac(6,13) = 1 ;

  jac(7,7)  = 1 ;
  jac(7,10) = 1 ;
  jac(7,14) = 1 ;
end

function H = hessian(x, sigma, L, auxdata)

  x2  = x(1) ;
  x3  = x(2) ;
  x4  = x(3) ;
  x5  = x(4) ;
  x6  = x(5) ;
  x7  = x(6) ;
  x8  = x(7) ;
  x9  = x(8) ;
  x10 = x(9) ;
  x11 = x(10) ;
  x12 = x(11) ;
  x13 = x(12) ;
  x14 = x(13) ;
  x15 = x(14) ;

  Hf = sparse(14,14) ;
  Hf(4,7) = -6.3 ;
  Hf(7,4) = -6.3 ;

  H2 = sparse(14,14) ;
  H2(3,6) = -1 ;
  H2(6,3) = -1;
  H2(4,6) = -(0.01*x10) ;
  H2(6,4) = -(0.01*x10);
  H2(4,9) = -x7*0.01 ;
  H2(9,4) = -x7*0.01 ;
  H2(6,9) = -(0.01*x5);
  H2(9,6) = -(0.01*x5);

  H3 = sparse(14,14) ;
  H3(1,8)  = -1 ;
  H3(8,1)  = -1 ;

  H4 = sparse(14,14) ;
  H4(1,8)  = -(0.13167 - 2*0.0067*x9) ;
  H4(8,1)  = -(0.13167 - 2*0.0067*x9) ;
  H4(4,11) = 1 ;
  H4(11,4) = 1 ;
  H4(8,8)  = x2*2*0.0067 ;

  H5 = sparse(14,14) ;
  H5(7,12) = 1 ;
  H5(12,7) = 1 ;
  H5(8,8)  = 0.01*(2*0.038) ;

  H6 = sparse(14,14) ;
  H6(9,13) = 1;
  H6(13,9) = 1;

  H7 = sparse(14,14) ;
  H7(10,14) = 1 ;
  H7(14,10) = 1 ;

  H  = tril(sparse(sigma * Hf + L(2)*H2 + L(3)*H3 + L(4)*H4 + L(5)*H5 + L(6)*H6 + L(7)*H7)) ;
end

function H = hessianstructure(auxdata)

  Hf = sparse(14,14) ;
  Hf(4,7) = 1;
  Hf(7,4) = 1;

  H2 = sparse(14,14) ;
  H2(3,6) = 1;
  H2(6,3) = 1;
  H2(4,6) = 1;
  H2(6,4) = 1;
  H2(4,9) = 1;
  H2(9,4) = 1;
  H2(6,9) = 1;
  H2(9,6) = 1;

  H3 = sparse(14,14) ;
  H3(1,8) = 1;
  H3(8,1) = 1;

  H4 = sparse(14,14) ;
  H4(1,8)  = 1;
  H4(8,1)  = 1;
  H4(4,11) = 1;
  H4(11,4) = 1;
  H4(8,8)  = 1;

  H5 = sparse(14,14) ;
  H5(7,12) = 1;
  H5(12,7) = 1;
  H5(8,8)  = 1;

  H6 = sparse(14,14) ;
  H6(9,13) = 1;
  H6(13,9) = 1;

  H7 = sparse(14,14) ;
  H7(10,14) = 1 ;
  H7(14,10) = 1 ;

  H = tril(sparse(Hf+H2+H3+H4+H5+H6+H7)) ;
end


function [output] = ft_transform_geometry(transform, input)

% FT_TRANSFORM_GEOMETRY applies a homogeneous coordinate transformation to a
% structure with geometric information, for example a volume conduction model for the
% head, gradiometer of electrode structure containing EEG or MEG sensor positions and
% MEG coil orientations, a head shape or a source model.
%
% The units in which the transformation matrix is expressed are assumed to be the
% same units as the units in which the geometric object is expressed. Depending on
% the input object, the homogeneous transformation matrix should be limited to a
% rigid-body translation plus rotation (MEG-gradiometer array), or to a rigid-body
% translation plus rotation plus a global rescaling (volume conductor geometry).
%
% Use as
%   output = ft_transform_geometry(transform, input)
% where the transform should be a 4x4 homogenous transformation matrix and the
% input data structure can be any of the FieldTrip data structures that
% describes geometrical data.
%
% See also FT_WARP_APPLY, FT_HEADCOORDINATES, FT_SCALINGFACTOR

% Copyright (C) 2011, Jan-Mathijs Schoffelen
%
% 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: ft_transform_geometry.m$

% flg rescaling check
allowscaling = ~ft_senstype(input, 'meg');

% determine the rotation matrix
rotation = eye(4);
rotation(1:3,1:3) = transform(1:3,1:3);

if any(abs(transform(4,:)-[0 0 0 1])>100*eps)
  ft_error('invalid transformation matrix');
end

if ~allowscaling
  % allow for some numerical imprecision
  if (abs(det(rotation))-1)>1e-6
    ft_error('only a rigid body transformation without rescaling is allowed');
  end
end

if allowscaling
  % FIXME build in a check for uniform rescaling probably do svd or so
  % FIXME insert check for nonuniform scaling, should give an error
end

tfields   = {'pos' 'pnt' 'o' 'coilpos' 'elecpos' 'optopos' 'chanpos' 'chanposold' 'nas' 'lpa' 'rpa' 'zpoint'}; % apply rotation plus translation
rfields   = {'ori' 'nrm'     'coilori' 'elecori' 'optoori' 'chanori' 'chanoriold'                           }; % only apply rotation
mfields   = {'transform'};           % plain matrix multiplication
recfields = {'fid' 'bnd' 'orig'};    % recurse into these fields
% the field 'r' is not included here, because it applies to a volume
% conductor model, and scaling is not allowed, so r will not change.

fnames = fieldnames(input);
for k = 1:numel(fnames)
  if ~isempty(input.(fnames{k}))
    if any(strcmp(fnames{k}, tfields))
      input.(fnames{k}) = apply(transform, input.(fnames{k}));
    elseif any(strcmp(fnames{k}, rfields))
      input.(fnames{k}) = apply(rotation, input.(fnames{k}));
    elseif any(strcmp(fnames{k}, mfields))
      input.(fnames{k}) = transform*input.(fnames{k});
    elseif any(strcmp(fnames{k}, recfields))
      for j = 1:numel(input.(fnames{k}))
        input.(fnames{k})(j) = ft_transform_geometry(transform, input.(fnames{k})(j));
      end
    else
      % do nothing
    end
  end
end
output = input;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION that applies the homogeneous transformation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [new] = apply(transform, old)
old(:,4) = 1;
new = old * transform';
new = new(:,1:3);


close all
clear

%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=24;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p4gamma2_0p4\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories

save('timecom24','nmpcplot1','impcplot1');
save('feasibility24','nmpcinf','impcinf');

distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF

%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end

function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;

%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];

%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
    0 1 0 0;
    0 0 1 0;
    0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
    x0(4,1)*sin(x0(3,1))*dt;
    0;
    0];
system.C =[0 0 0 0;
    0 0 0 0;
    1*dt 0 0 0;
    0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;

%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.4;
params_nmpc_dcbf.gamma2 = 0.4;

%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;

%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
    fprintf('Run NMPC-DCBF-24\n');
    controller_nmpc_dcbf_multiple24 = NMPCDCBF2(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
    controller_nmpc_dcbf_multiple24.obs = obs;
    controller_nmpc_dcbf_multiple24.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple24.tt;
end

function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 4;
gamma2 = 4;
nsim = ttsim;

tic;

% Constraints

umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10;  10;  10;  10];

% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];

% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];

% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
    x_0 = [x_0 x0new];
    x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end

abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);

% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];

% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();

% Check solver status
if ~strcmp(res.info.status, 'solved')
%    error('OSQP did not solve the problem!')
        t2=-1;
        return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);

for j = 1 : (K1-1)%Iterations for the first time-step
    storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
    storageu = ctrlu;
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return

x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;

for i = 1 : (nsim-1)%Iterations for the left time steps

   for j = 1 : K
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    x0 = ctrlx(1:nx);
    testx0 = [testx0 x0];
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
    storagex = ctrlx;
    storageu = ctrlu;
   end
   ctrl = ctrlu(1:nu);
   x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
   ctrlu = rewrite(ctrlu, nu, N);
   x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
   u_0 = transu(ctrlu, nu, N);
   impc(:, i+2) = x0;
end
end

% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
    for j = 1 : yy
        xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
    end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end

% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
    Acbfx(i, (i*nx)+1) = abc(1, (i+1));
    Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
    Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end

% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
 res = zeros(nxx, nyy);
 res(:,1) = x0;
 for i = 1 : (nyy -1)
     res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
 end
end

% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
 resu = zeros(nxx, nyy);
 for i = 1 : (nyy)
     resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
 end
end

% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end

% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
 x_0 = zeros(nx, N+1);
 x_0(:, 1) = x0;
 for i=1 : N
    u0 = ctrlu((i-1)*nu+1:i*nu);
    x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
    x_0(:, i + 1) = x0;
 end
end

% Get AA matrix
function AA = getaa(x0, dt)
 AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
 CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
   x0 = x_0(:,i+1);
   AA = getaa(x0, dt);
   Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
    u0 = u_0(:,i);
    x0 = x_0(:,i);
    x1 = x_0(:,i+1);
    CC = getcc(x0, x1, u0, dt);
    Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
    Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
    Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
    Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
    Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
    Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
    Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
    Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
    lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end


function varargout = removeMeshVertices(vertices, faces, indsToRemove)
%REMOVEMESHVERTICES Remove vertices and associated faces from a mesh.
%
%   [V2, F2] = removeMeshVertices(VERTS, FACES, VERTINDS)
%   Removes the vertices specified by the vertex indices VERTINDS, and
%   remove the faces containing one of the removed vertices.
%
%   Example
%     % remove some vertices from a soccerball polyhedron
%     [v, f] = createSoccerBall;
%     plane = createPlane([.6 0 0], [1 0 0]);
%     indAbove = find(~isBelowPlane(v, plane));
%     [v2, f2] = removeMeshVertices(v, f, indAbove);
%     drawMesh(v2, f2);
%     axis equal; hold on;
%
%   See also
%     meshes3d, trimMesh

% ------
% Author: David Legland
% E-mail: david.legland@nantes.inra.fr
% Created: 2016-02-03, using Matlab 8.6.0.267246 (R2015b)
% Copyright 2016-2022 INRA - Cepia Software Platform

% parse inputs
if nargin == 2
    indsToRemove = faces;
    [vertices, faces] = parseMeshData(vertices);
end

% create array of indices to keep
nVertices = size(vertices, 1);
newInds = (1:nVertices)';
newInds(indsToRemove) = [];

% create new vertex array
vertices2 = vertices(newInds, :);

% compute map from old indices to new indices
oldNewMap = zeros(nVertices, 1);
for iIndex = 1:size(newInds, 1)
   oldNewMap(newInds(iIndex)) = iIndex;
end

% change labels of vertices referenced by faces
if isnumeric(faces)
    faces2 = oldNewMap(faces);
    if size(faces2,2)==1; faces2=faces2'; end
    % keep only faces with valid vertices
    faces2 = faces2(sum(faces2 == 0, 2) == 0, :);
elseif iscell(faces)
    faces2 = cell(1, length(faces));
    for iFace = 1:length(faces)
         newFace = oldNewMap(faces{iFace});
         % add the new face only if all vertices are valid
         if ~any(newFace == 0)
             faces2{iFace} = newFace;
         end
    end

    % remove empty faces
    faces2 = faces2(~cellfun(@isempty, faces2));
end

% format output arguments
varargout = formatMeshOutput(nargout, vertices2, faces2);


function init(obj, varargin)
% init(obj, varargin)
%
% Initialize the CalvinNN network with the default options.
% Also starts up GPU pool.
%
% Copyright by Holger Caesar, 2016

defnnOpts.expDir = fullfile('data', 'exp');
defnnOpts.continue = true;
defnnOpts.batchSize = 2;
defnnOpts.numSubBatches = 2;
defnnOpts.gpus = [];
defnnOpts.numEpochs = 16;
defnnOpts.learningRate = [repmat(1e-3, [1, 12]), repmat(1e-4, [1, 4])];
defnnOpts.weightDecay = 0.0005;
defnnOpts.momentum = 0.9;
defnnOpts.derOutputs = {'objective', 1};
defnnOpts.lossFnObjective = 'softmaxlog'; % Default is softmax
defnnOpts.extractStatsFn = @CalvinNN.extractStats;
defnnOpts.testFn = @(imdb, nnOpts, net, inputs, batchInds) error('Error: Test function not implemented'); % function used at test time to evaluate performance
defnnOpts.misc = struct(); % fields used by custom layers are stored here
defnnOpts.plotEval = true;
defnnOpts.plotAccuracy = true;

% Network options
defnnOpts.convertToTrain = true;

% Fast R-CNN options
defnnOpts.fastRcnn = true;
defnnOpts.fastRcnnParams = true; % learning rates and weight decay
defnnOpts.misc.roiPool.use = true;
defnnOpts.misc.roiPool.freeform.use = false;
defnnOpts.bboxRegress = true;

% Merge input settings with default settings
nnOpts = vl_argparse_old(defnnOpts, varargin, 'nonrecursive');

% Check settings
assert(numel(nnOpts.learningRate) == 1 || numel(nnOpts.learningRate) == nnOpts.numEpochs);

% Do not create directory in evaluation mode
if ~exist(nnOpts.expDir, 'dir') && ~isempty(nnOpts.expDir),
    mkdir(nnOpts.expDir);
end

% Setup GPU
numGpus = numel(nnOpts.gpus);
assert(numGpus <= 1);
if numGpus == 1,
    gpuDevice(nnOpts.gpus);
end

% Set new fields
obj.nnOpts = nnOpts;


%%Generate a simualted trajectory
%rstat: empty --> randomize (also return the seed in case it may be needed for debugging)
%       else --> use rstat as the seed (must be either a postive int or an rstat returned by rsg)
%dir_name : Directory to write outputs (must include trailing slash)
%ini_pva :[pos_in_nav(lLh), vel_in_body, body_nav_euler];
%out_typ :[[simulation_freq_multiplier imu_output_freq];[1 gps_freq]; [2 odo_freq]]
%sim_mode :
%   0:nav frame simulation,
%   1:simulation in a planar frame with constant g (position output in meters) (ini_pos must also be in meters)
%mot_def : Definition of motions. Each row defines a segment.
%   mot_def(:,1)=motion type (see the code for the options)
%   mot_def(:,2:5)=parameters of the motion (see the code)
%   mot_def(:,6)=maximum time for the given segment
%sen_conf: configuration for additional individual sensors (senconf=[la_in_master;euler_in_master])
%imu_conf: configuration for additional imus (imuconf=[la_in_master;euler_in_master])

%Output Files:
%mimu.bin: imu_data=[out_ind, acc, rotation_rate] (time=out_ind/imu_freq)
%mnav.bin: true_nav_data=[out_ind, pos_n or pos_dn, vel_b;att_n] (sim_mode determines position)
function rstat=PathGen(dir_name, ini_pva, mot_def, out_typ, sim_mode, sen_conf, imu_conf, vib_param, rstat)

if (rstat(1)~=0)
    randn('state',rstat);
else
    rstat=randn('state');
end

DEBUG=0;    %runs an additional conventional ins to see how compatible the trajectory generator and ins outputs are. (usually required to see the effect numerical approximations under vibration)

%% Frequencies
outfreq=out_typ(1,2);    %imu output frequency
intmul=out_typ(1,1);      %simulation freq multiplier. simulation freq=outfreq*intmul
dt=1/outfreq/intmul;

%%Additional Sensor configurations
nsen=0;     %number of additional sensor units
if (~isempty(sen_conf))    %there exist additional sensors
    nsen=size(sen_conf,1);
    sen_la=zeros(3,nsen);
    sen_or=zeros(3,3,nsen); %dcm from master to sensor
    sen_typ=sen_conf(:,7);
    for in=1:nsen
        sen_la(:,in)=sen_conf(in,1:3); %lever arm in the master body frame
        sen_or(:,:,in)=euler2dcm_v000(sen_conf(in,4:6)); %transformation matrix from sensor to master (euler angles defined in master);
    end
end

%%Additional imu configurations;
nimu=0;     %number of additional imu units
if (~isempty(imu_conf))    %there exist additional sensors
    nimu=size(imu_conf,1);
    imu_la=zeros(3,nimu);
    imu_or=zeros(3,3,nimu); %dcm from master to sensor
    imu_vib=zeros(3,3,nimu);
    for in=1:nimu
        imu_la(:,in)=imu_conf(in,1:3); %lever arm in the master body frame
        imu_or(:,:,in)=euler2dcm_v000(imu_conf(in,4:6)); %transformation matrix from sensor to master (euler angles defined in master);
        imu_vib(:,:,in)=eye(3);     %default value=no vibration
    end
end

%%vibration parameters for additional imus
if (nimu>0)
    vib_state=zeros(3,nimu);
    vib_model=zeros(2,nimu);
    if (~isempty(vib_param))
        for in=1:nimu
            vib_model(:,in)=vib_param(in,1:2)';
            vib_state(3,in)=randn(1)*vib_param(in,3);     %initial vibration angles
            imu_vib(:,:,in)=euler2dcm_v000(vib_state(:,in));
        end
    end
end

%% Path Gen Command Filter and feedback parameters (somehow arbitrary)
INP_FILT_A=eye(3)*0.9;
INP_FILT_B=eye(3)-INP_FILT_A;
att_n_dot=zeros(3,1);
vel_b_dot=zeros(3,1);

%% Open output files
fmimu=fopen([dir_name 'mimu.bin'],'wb');  %sensor outputs
fmnav=fopen([dir_name 'mnav.bin'],'wb');  %master navigation solution

if (nimu>0) %for each additional imus, create additional navigation and imu files
    fsnav=zeros(nimu,1);    %navigation solution files (for each full imu)
    fsimu=zeros(nimu,1);    %imu files
    for in=1:nimu
        fsnav(in)=fopen([dir_name 'snav' int2str(in) '.bin'],'wb');
        fsimu(in)=fopen([dir_name 'simu' int2str(in) '.bin'],'wb');
    end
end

nout=size(out_typ,1);
for in=2:nout    %measurement files
    switch out_typ(in,1)
        case {1}
            fgps=fopen([dir_name 'gps.bin'],'wb');
        case {2}
            fodo=fopen([dir_name 'odo.bin'],'wb');
    end
end

%% convert times into indeces
for in=2:nout;
    out_typ(in,2)=intmul*round(outfreq/out_typ(in,2));
end
for in=1:size(mot_def,1)
    mot_def(in,6)=round(mot_def(in,6)*outfreq*intmul);
end

%%%%%%%%%%% Start Computations %%%%%%%%%%%%%%%
sim_cnt=0;
fflag=1;
acc_tot=zeros(3,1);
gyro_tot=zeros(3,1);
odo_dist=0;

%master navigator states
att_n=ini_pva(:,3);
Cbn=euler2dcm_v000(att_n);
pos_n=ini_pva(:,1);
pos_dn=zeros(3,1);  %total position change
vel_b=ini_pva(:,2);
vel_n=Cbn*vel_b;
[Rn, Re, g, sL, cL, WIE]=geoparam_v000(pos_n);

%Write initial values to the file
fwrite(fmimu,[0;zeros(3,1);zeros(3,1);zeros(nsen,1)],'double');
if (sim_mode==0)
    vr_a=[0;pos_n+pos_dn;vel_b;att_n];
else
    vr_a=[0;pos_dn;vel_b;att_n];
end
fwrite(fmnav, vr_a,'double');

if (nimu>0)
    if (sim_mode==0)
        [spos_n, svel_b, sCsn, simu]=la_imu_v000(zeros(3,1), zeros(3,1), zeros(3,1), pos_n, vel_b, Cbn, imu_la, imu_or, 0, zeros(3,1), imu_vib);
    elseif (sim_mode==1)
        [spos_n, svel_b, sCsn, simu]=la_imu_v000(zeros(3,1), zeros(3,1), zeros(3,1), pos_dn, vel_b, Cbn, imu_la, imu_or, 1, zeros(3,1), imu_vib);
    end
    for in1=1:nimu
        satt_n=dcm2euler_v000(sCsn(:,:,nimu));
        fwrite(fsnav(in1), [0;spos_n(:,in1);svel_b(:,in1); satt_n], 'double');
        fwrite(fsimu(in1), [0;simu], 'double');
    end
end

%%variables for debugging
if (DEBUG)
    %strapdown states (mostly for debugging purposes)
    strap_Cbn=Cbn;
    strap_vel_b=vel_b;
    if (sim_mode==0)
        strap_pos_n=pos_n;
    elseif (sim_mode==1)
        strap_pos_n=pos_dn;
    end

    if (nimu>0)
        strap_spos_n=spos_n;
%         for in=1:nimu       %!!!THIS DOES DOT SEEM OK.!!!!
%             strap_svel_b=svel_b(:,in);
%         end
        strap_svel_b=svel_b;
        strap_sCsn=sCsn;
    end

    sr_a=sum(mot_def(:,6)); %maximum number of outputs
    deb_vel=zeros(3,sr_a);
    deb_pos=zeros(3,sr_a);
    deb_att=zeros(3,sr_a);
    deb_ctr=0;
end

%Start generation of trajectory
for in=1:size(mot_def,1)
    mmod=mot_def(in,1);

    if (mmod==1)
        %Determine the inputs directly
        att_n_dot_com=[mot_def(in,2);mot_def(in,3);mot_def(in,4)];
        vel_b_dot_com=[mot_def(in,5);0;0];
    elseif mmod==2    %new abs. att and vel values to reach
        %determine the commands
        att_n_com=[mot_def(in,2);mot_def(in,3);mot_def(in,4)];
        vel_b_com=[mot_def(in,5);0;0];
    elseif mmod==3    %rel att and vel changes
        att_n_com=[mot_def(in,2)+att_n(1);mot_def(in,3)+att_n(2);mot_def(in,4)+att_n(3)];
        vel_b_com=vel_b+[mot_def(in,5);0;0];
    elseif mmod==4    %abs attitude, rel vel
        att_n_com=[mot_def(in,2);mot_def(in,3);mot_def(in,4)];
        vel_b_com=vel_b+[mot_def(in,5);0;0];
    elseif mmod==5    %rel att, abs vel
        att_n_com=[mot_def(in,2)+att_n(1);mot_def(in,3)+att_n(2);mot_def(in,4)+att_n(3)];
        vel_b_com=[mot_def(in,5);0;0];
    end

    att_n_com_filt=att_n;
    vel_b_com_filt=vel_b;

    seg_cnt_lim=sim_cnt+mot_def(in,6);
    seg_next=0;

    while ((sim_cnt<seg_cnt_lim) && ~seg_next)
        %determine the inputs
        if (mmod==1)
            %filter the input
            att_n_dot=INP_FILT_A*att_n_dot+INP_FILT_B*att_n_dot_com;
            vel_b_dot=INP_FILT_A*vel_b_dot+INP_FILT_B*vel_b_dot_com;
        else
            %filter the command
            att_n_com_filt=INP_FILT_A*att_n_com_filt+INP_FILT_B*att_n_com;
            vel_b_com_filt=INP_FILT_A*vel_b_com_filt+INP_FILT_B*vel_b_com;

            %close the loop
            att_n_dot=att_n_com_filt-att_n;
            vel_b_dot=vel_b_com_filt-vel_b;

            if (norm(att_n_dot)<1e-4 && norm(vel_b_dot)<1e-4)
                seg_next=1;
            end
        end

        %Compute the master imu solution
        [acc, gyro, vel_n_dot, pos_n_dot, Cbn_dot]=comp_master(pos_n+pos_dn, vel_b, att_n, Cbn, vel_b_dot, att_n_dot, sim_mode, g);

        %update master results
        acc_tot=acc_tot+acc;
        gyro_tot=gyro_tot+gyro;
        pos_dn=pos_dn+pos_n_dot*dt;
        att_n=att_n+att_n_dot*dt;
        %Cbn=euler2dcm_v000(att_n);
        Cbn=Cbn+Cbn_dot*dt;
        vel_b=vel_b+vel_b_dot*dt;
        vel_n=Cbn*vel_b;
%         vel_n=vel_n+vel_n_dot*dt;
%         vel_b=Cbn'*vel_n;

        %%odometer resuts
        odo_vel=vel_b;
        odo_dist=odo_dist+norm(vel_b)*dt;

        %update simulation counter
        sim_cnt=sim_cnt+1;

        %write the results
        if (mod(sim_cnt,intmul)==0)
            %%average gyro and acc for the output interval
            gyro_avg=gyro_tot/intmul;
            acc_avg=acc_tot/intmul;

            if (fflag)  %only once: set gyro derivative to zero, write initial values to the files
                fflag=0;
                gyro_pre=gyro_avg;
                acc_pre=acc_avg;
            end

            %%Compute the sensor outputs
            gyro_der=(gyro_avg-gyro_pre)*outfreq;
            if (nsen>0)
                sensor_out=la_sen_v000(acc_avg, gyro_avg, gyro_der, sen_la, sen_or, sen_typ);
            else
                sensor_out=[];
            end
            fwrite(fmimu,[(sim_cnt/intmul);acc_avg;gyro_avg;sensor_out],'double');
            if (sim_mode==0)
                vr_a=[(sim_cnt/intmul);pos_n+pos_dn;vel_b;att_n];
            else
                vr_a=[(sim_cnt/intmul);pos_dn;vel_b;att_n];
            end
            fwrite(fmnav, vr_a,'double');

            %Compute the slave imu outputs
            if (nimu>0)
                %update the vibration
                [gyro_vib, vib_state, imu_vib_new]=update_vib(vib_state, vib_model, imu_vib, imu_or, 1/outfreq);

                %compute the slave results
                if (sim_mode==0)
                    [spos_n, svel_b, sCsn, simu]=la_imu_v000(acc_avg, gyro_avg, gyro_der, pos_n+pos_dn, vel_b, Cbn, imu_la, imu_or, 0, gyro_vib, imu_vib);
                elseif (sim_mode==1)
                    [spos_n, svel_b, sCsn, simu]=la_imu_v000(acc_avg, gyro_avg, gyro_der, pos_dn, vel_b, Cbn, imu_la, imu_or, 1, gyro_vib, imu_vib);
                end
                for in1=1:nimu
                    satt_n=dcm2euler_v000(sCsn(:,:,nimu));
                    fwrite(fsnav(in1), [(sim_cnt/intmul);spos_n(:,in1);svel_b(:,in1);satt_n(:,in1)], 'double');
                    fwrite(fsimu(in1), [(sim_cnt/intmul);simu(:,in1)], 'double');
                end
                imu_vib=imu_vib_new;
            end

            %prepare for the next cycle
            gyro_pre=gyro_avg;
            gyro_tot=zeros(3,1);
            acc_tot=zeros(3,1);

            %%debug
            if (DEBUG)
                %%run the sptradown
                if (sim_mode==0)
                    [strap_Cbn, strap_vel_b, strap_pos_n]=strapdown_bd_dcm_v000(strap_Cbn, strap_vel_b, strap_pos_n, acc_avg, gyro_avg, 1/outfreq);
                    for in1=1:nimu
                        [strap_sCsn(:,:,in1), strap_svel_b(:,in1), strap_spos_n(:,in1)]=strapdown_bd_dcm_v000(strap_sCsn(:,:,in1), strap_svel_b(:,in1), strap_spos_n(:,in1), simu(1:3,in1), simu(4:6,in1), 1/outfreq);
                    end
                elseif (sim_mode==1)
                    [strap_Cbn, strap_vel_b, strap_pos_n]=strapdown_pln_dcm_v000(strap_Cbn, strap_vel_b, strap_pos_n, acc_avg, gyro_avg, g, 1/outfreq, 1);
                    for in1=1:nimu
                        [strap_sCsn(:,:,in1), strap_svel_b(:,in1), strap_spos_n(:,in1)]=strapdown_pln_dcm_v000(strap_sCsn(:,:,in1), strap_svel_b(:,in1), strap_spos_n(:,in1), simu(1:3,in1), simu(4:6,in1),g, 1/outfreq,1);
                    end
                end

                deb_ctr=deb_ctr+1;
%                 deb_vel(:,deb_ctr)=strap_svel_b-svel_b;
%                 deb_pos(:,deb_ctr)=strap_spos_n-spos_n;
%                 deb_att(:,deb_ctr)=dcm2euler_v000(strap_sCsn(:,:,1)*sCsn');
                deb_vel(:,deb_ctr)=strap_vel_b-vel_b;
                deb_pos(:,deb_ctr)=strap_pos_n-pos_dn-pos_n;
                deb_att(:,deb_ctr)=dcm2euler_v000(strap_Cbn(:,:,1)*Cbn');
            end

        end

        %%measurement files
        if (nout>1)
            for sr_a=2:nout;
                switch out_typ(sr_a,1)
                    case {1}
                        if (mod(sim_cnt,out_typ(sr_a,2))==0)
                            if (sim_mode==0)
                                vr_a=[(sim_cnt/intmul);vel_n;pos_n+pos_dn];
                            elseif(sim_mode==1)
                                vr_a=[(sim_cnt/intmul);vel_n;pos_dn];
                            end
                            fwrite(fgps,vr_a,'double');
                        end
                    case {2}
                        if (mod(sim_cnt,out_typ(sr_a,2))==0)
                            fwrite(fodo, [(sim_cnt/intmul);odo_vel;odo_dist],'double');
                        end
                end
            end
        end
    end
end
if (DEBUG)
    deb_vel(:,deb_ctr)
    deb_pos(:,deb_ctr)
    deb_att(:,deb_ctr)
end
fclose('all');

%vel_typ==1 -> b, vel_typ==2 -> n
function [acc, gyro, vel_n_dot, pos_n_dot, Cbn_dot]=comp_master(pos_n, vel_b, att_n, Cbn, vel_b_dot, att_n_dot, styp, g)
%velocity in n
vel_n=Cbn*vel_b;

%%Calculate Spherical Earth Parameters and gravity
if (styp==0)
    [Rn, Re, g, sL, cL, WIE]=geoparam_v000(pos_n);
    Rn_eff=Rn+pos_n(3);
    Re_eff=Re+pos_n(3);
    gravity=[0;0;-g];
elseif (styp==1)
    gravity=[0;0;-g];
end

%compute w_nb_n (rotation rate of "b" wrt "n" defined in "n")
sh=sin(att_n(3));ch=cos(att_n(3));
w_nb_n=zeros(3,1);
w_nb_n(1)=-sh*att_n_dot(2)+Cbn(1,1)*att_n_dot(1);
w_nb_n(2)=ch*att_n_dot(2)+Cbn(2,1)*att_n_dot(1);
w_nb_n(3)=att_n_dot(3)+Cbn(3,1)*att_n_dot(1);

%%compute w_en_n & w_ie_n
if (styp==0)
    w_en_n=zeros(3,1);
    w_en_n(1)=vel_n(2)/(Re_eff);
    w_en_n(2)=-vel_n(1)/(Rn_eff);
    w_en_n(3)=-vel_n(2)*(sL/cL)/(Re_eff);

    w_ie_n=zeros(3,1);
    w_ie_n(1)=+WIE*cL;
    w_ie_n(2)=0;
    w_ie_n(3)=-WIE*sL;
elseif(styp==1)
    w_en_n=zeros(3,1);
    w_ie_n=zeros(3,1);
end

%attitude derivative
Cbn_dot=Cbn*skew(Cbn'*w_nb_n);

%%velocity derivative
vel_n_dot=Cbn*vel_b_dot+skew(w_nb_n)*vel_n;

%%position derivative
if (styp==0)
    pos_n_dot=zeros(3,1);
    pos_n_dot(1)=vel_n(1)/(Rn_eff);
    pos_n_dot(2)=vel_n(2)/(Re_eff)/cL;
    pos_n_dot(3)=-vel_n(3);
elseif (styp==1)
    pos_n_dot=zeros(3,1);
    pos_n_dot(1)=vel_n(1);
    pos_n_dot(2)=vel_n(2);
    pos_n_dot(3)=vel_n(3);
end

%%gyroscope output
gyro=Cbn'*(w_nb_n+w_en_n+w_ie_n);

% %%Acceleration output
% acc=Cbn'*(vel_ned_dot+(skew(w_en_n)+2*skew(w_ie_n))*vel_ned+gravity);

%%Acceleration output
vr_a=Cbn'*w_ie_n;
acc=vel_b_dot+skew(vr_a+gyro)*vel_b+Cbn'*gravity;

function [gyro_vib, vib_state_new, imu_vib_new]=update_vib(vib_state, vib_model, imu_vib, imu_or, dt)
nimu=size(vib_model,2);
vib_state_new=zeros(3,nimu);
imu_vib_new=zeros(3,3,nimu);
gyro_vib=zeros(3,nimu);
for in=1:nimu
    %compute vibration induced rotation rate in master frame
    Crig=imu_or(:,:,in);    %rigid Csm
    Cvib=imu_vib(:,:,in);   %vibration Cs's , complete rotation Cs'm=Crig*Cvib
    vib_state_new(:,in)=vib_model(1,in)*vib_state(:,in)+[0;0; vib_model(2,in)*randn(1)];
    gyro_vib(:,in)=Crig*(vib_state_new(:,in)-vib_state(:,in))/dt;

    %update the vibration
    rot=Cvib'*(vib_state_new(:,in)-vib_state(:,in));
    rot_norm=norm(rot);
    sr_a=1-(rot_norm^2/6)+(rot_norm^4/120);
    sr_b=(1/2)-(rot_norm^2/24)+(rot_norm^4/720);
    mx_a=eye(3)+sr_a*skew(rot)+sr_b*skew(rot)*skew(rot);
    Cvib=Cvib*mx_a;

    imu_vib_new(:,:,in)=Cvib;
end


function [ labels, valid_ids, filenames  ] = extract_FERA2011_labels( FERA2011_dir, recs, aus )
%EXTRACT_SEMAINE_LABELS Summary of this function goes here
%   Detailed explanation goes here

    num_files = numel(recs);

    % speech invalidates lower face AUs
    labels = cell(num_files, 1);
    valid_ids = cell(num_files, 1);
    filenames = cell(num_files, 1);

    file_id = 1;

    for i=1:numel(recs)

        file = [FERA2011_dir, '/', recs{i}, '/', recs{i}, '-au.dat'];

        [~, filename,~] = fileparts(file);
        filenames{file_id} = filename;

        delimiter = {'   '};
        formatSpec = '%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%[^\n\r]';

        %% Open the text file.
        fileID = fopen(file,'r');

        %% Read columns of data according to the format.
        data = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'MultipleDelimsAsOne', true, 'TextType', 'string',  'ReturnOnError', false);
        data = [data{1:end-1}];

        %% Close the text file.
        fclose(fileID);

        %data = dlmread(file, ' '); %import annotations for one video file

        speech = data(:,end);

        labels{file_id} = data(:, aus);

        % Finding the invalid regions
        if(aus(1) >= 10)
            valid = speech == 0;
        else
            valid = true(size(speech,1), 1);
        end

        % all indices in SEMAINE are valid
        valid_ids{file_id} = valid;

        file_id = file_id + 1;
    end

    labels = labels(1:file_id-1);
    valid_ids = valid_ids(1:file_id-1);
    filenames = filenames(1:file_id-1);

end