%ReplicationInitiationDuration
%
% Author: Jonathan Karr, jkarr@stanford.edu
% Affiliation: Covert Lab, Department of Bioengineering, Stanford University
% Last Updated: 9/12/2010
classdef ReplicationInitiationDuration
    methods (Static)
        %inputFilePath is a path to .mat files containing replication initiation
        %durations generated by
        %edu.stanford.covert.cell.sim.process.ReplicationInitiation_Test.meanDur
        %ation
        function run(inputFilePath, outputFileName)
            import edu.stanford.covert.cell.sim.analysis.ReplicationInitiationDuration;
            import edu.stanford.covert.cell.sim.util.PrintUtil;

            %load data
            [durations, constants] = ReplicationInitiationDuration.loadDurations(inputFilePath);

            %statistics
            [vals, ~, means, stds, mins, maxs, meds, p10, p25, p75, p90] = ...
                ReplicationInitiationDuration.calculateStatistics(durations, constants);
            colLabels = {'Mean DnaA Copy Number', 'Site Cooperativity', 'State Cooperativity', 'No. DnaA Boxes', ...
                'Mean', 'Std. Dev', 'Min', '10%', '25%', 'Median', '75%', '90%', 'Max'};
            if nargin == 1
                PrintUtil.printToStdIO(num2cell([vals means stds mins p10 p25 meds p75 p90 maxs]), colLabels);
            else
                PrintUtil.printToFile(num2cell([vals means stds mins p10 p25 meds p75 p90 maxs]), ...
                    colLabels, [outputFileName '.xls'], 'Statistics')
            end

            %plots
            if nargin == 2, [axesHandle, figHandle] = edu.stanford.covert.cell.sim.util.PlotUtil.newAxesHandle(); end

            if nargin == 1, [axesHandle, figHandle] = edu.stanford.covert.cell.sim.util.PlotUtil.newAxesHandle(); end
            cla(axesHandle);
            tfs = ismember(constants(:, [1 3 4]), constants(1, [1 3 4]), 'rows');
            ReplicationInitiationDuration.plotScatterPlot(axesHandle, durations(tfs), constants(tfs, :));
            if nargin == 2, saveas(figHandle, [outputFileName '-ScatterPlot.pdf']); end

            if nargin == 1, [axesHandle, figHandle] = edu.stanford.covert.cell.sim.util.PlotUtil.newAxesHandle(); end
            cla(axesHandle);
            tfs = ismember(constants, constants(1, :), 'rows');
            ReplicationInitiationDuration.plotHistogram(axesHandle, durations(tfs), constants(tfs, :));
            if nargin == 2, saveas(figHandle, [outputFileName '-Histogram.pdf']); end

            if nargin == 1, [axesHandle, figHandle] = edu.stanford.covert.cell.sim.util.PlotUtil.newAxesHandle(); end
            cla(axesHandle);
            ReplicationInitiationDuration.plotDistributions(axesHandle, durations, constants);
            if nargin == 2, saveas(figHandle, [outputFileName '-Distributions.pdf']); end

            if nargin == 1, [axesHandle, figHandle] = edu.stanford.covert.cell.sim.util.PlotUtil.newAxesHandle(); end
            cla(axesHandle);
            ReplicationInitiationDuration.plotMeanVsStd(axesHandle, durations, constants);
            if nargin == 2, saveas(figHandle, [outputFileName '-MeanVsStd.pdf']); end

            if nargin == 2, close(figHandle); end
        end
    end

    methods (Static)
        function [durations, constants] = loadDurations(filePath)
            fileNames = dir([filePath filesep '*.mat']);
            durations = zeros(0, 1);
            constants = zeros(0, 4);
            path = fileparts([filePath filesep]);
            for i = 1:size(fileNames, 1)
                [~, parameters] = regexp(fileNames(i).name, ...
                    'ReplicationInitiationDuration-(?<meanDnaACopyNumber>\d+)-(?<siteCooperativity>\d+\.\d+)-(?<stateCooperativity>\d+\.\d+)-(?<nDnaABoxes>\d+)\.mat', 'match', 'names');

                data = load([path filesep fileNames(i).name]);
                durations = [durations; data.durations]; %#ok<AGROW>
                constants = [constants; repmat([...
                    str2double(parameters.meanDnaACopyNumber) ...
                    str2double(parameters.siteCooperativity) ...
                    str2double(parameters.stateCooperativity) ...
                    str2double(parameters.nDnaABoxes) ...
                    ], numel(data.durations), 1)]; %#ok<AGROW>
            end

            durations = durations / 3600;
        end

        function [vals, cnts, means, stds, mins, maxs, meds, p10, p25, p75, p90] = ...
                calculateStatistics(durations, constants)
            durationBins = 0:2:20;

            vals = unique(constants, 'rows');
            cnts = zeros(numel(durationBins), size(vals, 1));

            means = zeros(size(vals, 1), 1);
            stds = zeros(size(vals, 1), 1);
            mins = zeros(size(vals, 1), 1);
            maxs = zeros(size(vals, 1), 1);
            meds = zeros(size(vals, 1), 1);
            p25 = zeros(size(vals, 1), 1);
            p75 = zeros(size(vals, 1), 1);
            p90 = zeros(size(vals, 1), 1);
            p10 = zeros(size(vals, 1), 1);

            durations = min(durations, 3*23825/3600);

            for i = 1:size(vals, 1)
                tmpDurations = durations(ismember(constants, vals(i, :), 'rows'), 1);
                cnts(:,i) = histc(tmpDurations, durationBins);
                means(i) = mean(tmpDurations);
                stds(i) = std(tmpDurations);
                mins(i) = min(tmpDurations);
                maxs(i) = max(tmpDurations);
                meds(i) = median(tmpDurations);
                p25(i) = quantile(tmpDurations, 0.25);
                p75(i) = quantile(tmpDurations, 0.75);
                p90(i) = quantile(tmpDurations, 0.90);
                p10(i) = quantile(tmpDurations, 0.10);
            end
        end
    end

    %plots
    methods (Static)
        function plotScatterPlot(axesHandle, durations, constants)
            if isempty(axesHandle)
                axesHandle = edu.stanford.covert.cell.sim.util.PlotUtil.newAxesHandle();
            end

            plot(axesHandle, constants(:, 2), durations, '.');
            xlabel(axesHandle, 'Site Cooperativity Constant', 'fontsize', 14);
            ylabel(axesHandle, 'Replication Initiation Time (h)', 'fontsize', 14);
            title(axesHandle, sprintf('DnaA %d, State %f, DnaA Boxes %d', ...
                constants(1), constants(3), constants(4)), 'fontsize', 14);
        end

        function plotHistogram(axesHandle, durations, constants)
            if isempty(axesHandle)
                axesHandle = edu.stanford.covert.cell.sim.util.PlotUtil.newAxesHandle();
            end

            hist(axesHandle, durations);
            ylabel(axesHandle, 'Frequency', 'fontsize', 14);
            xlabel(axesHandle, 'Replication Initiation Time (h)', 'fontsize', 14);
            title(axesHandle, sprintf('DnaA %d, State %f, DnaA Boxes %d', ...
                constants(1), constants(3), constants(4)), 'fontsize', 14);
        end

        function plotDistributions(axesHandle, durations, constants)
            if isempty(axesHandle)
                axesHandle = edu.stanford.covert.cell.sim.util.PlotUtil.newAxesHandle();
            end

            [vals, cnts] = edu.stanford.covert.cell.sim.analysis.ReplicationInitiationDuration.calculateStatistics(...
                durations, constants);

            colors = [
                1.00 0 0;
                0.75 0 0;
                0.50 0 0;
                0.25 0 0;
                0 0 0;
                0 0 0.25;
                0 0 0.50;
                0 0 0.75;
                0 0 1];

            h = plot(axesHandle, (0:2:20)', cnts);
            for i = 1:size(vals, 1)
                set(h(i), 'color', colors(mod(i-1, size(colors, 1))+1, :));
            end
            legend(h, cellfun(@(val1, val2, val3, val4) sprintf('%d %.2f %.2f %d', val1, val2, val3, val4), ...
                num2cell(vals(:, 1)), ...
                num2cell(vals(:, 2)), ...
                num2cell(vals(:, 3)), ...
                num2cell(vals(:, 4)), ...
                'UniformOutput', false), 'Location', 'BestOutside');
            ylabel(axesHandle, 'Frequency', 'fontsize', 14);
            xlabel(axesHandle, 'Replication Initiation Time (h)', 'fontsize', 14);
            ylim(axesHandle, [0 max(cnts(:)) + eps]);
        end

        function plotMeanVsStd(axesHandle, durations, constants)
            if isempty(axesHandle)
                axesHandle = edu.stanford.covert.cell.sim.util.PlotUtil.newAxesHandle();
            end

            [vals, ~, means, stds] = edu.stanford.covert.cell.sim.analysis.ReplicationInitiationDuration.calculateStatistics(...
                durations, constants);

            vals3 = unique(vals(:, 3));
            markerSizes = repmat([10 18 8 12 24]', ceil(numel(vals3)/5), 1);
            markerSizes = markerSizes(1:numel(vals3));
            vals2 = unique(vals(:, 2));
            markerStyles = repmat(('.+*oxsdv^<')', ceil(numel(vals2)/10), 1);
            markerStyles = markerStyles(1:numel(vals2));
            vals1 = unique(vals(:, 1));
            colors = repmat([
                1 0 0
                0 0 0
                0 0 1
                0.75 0 0
                0.50 0 0
                0.25 0 0
                0 0 0.25
                0 0 0.50
                0 0 0.75
                ], ceil(numel(vals1)/9), 1);
            colors = colors(1:numel(vals1), :);

            hold(axesHandle, 'on');
            for i = 1:size(vals, 1)
                h(i) = plot(axesHandle, means(i), stds(i), '.', ...
                    'MarkerSize', markerSizes(ismember(vals3, vals(i, 3))), ...
                    'Marker', markerStyles(ismember(vals2, vals(i, 2))), ...
                    'Color', colors(ismember(vals1, vals(i, 1)), :));
            end
            legend(h, cellfun(@(val1, val2, val3, val4) sprintf('%d %.2f %.2f %d', val1, val2, val3, val4), ...
                num2cell(vals(:, 1)), ...
                num2cell(vals(:, 2)), ...
                num2cell(vals(:, 3)), ...
                num2cell(vals(:, 4)), ...
                'UniformOutput', false), 'Location', 'BestOutside');
            xlim(axesHandle, [min(means) max(means)]+[-1 1]*0.1*range(means));
            ylim(axesHandle, [min(stds) max(stds)]+[-1 1]*0.1*range(stds));
            ylabel(axesHandle, 'Duration standard deviation (h)', 'fontsize', 14);
            xlabel(axesHandle, 'Mean duration (h)', 'fontsize', 14);
        end
    end
end


function [rtk,D,nb]=ddmat(rtk,D)

global glc
nf=rtk.NF; na=rtk.na;nb=0;
MAXSAT=glc.MAXSAT;

for i=1:MAXSAT
    for j=1:glc.NFREQ
        rtk.sat(i).fix(j)=0;
    end
end

for i=1:na
   D(i,i)=1;
end

for m=1:5
    nofix=(m==2&&rtk.opt.glomodear==0)||(m==4&&rtk.opt.bdsmodear==0);

    k=na;
    for f=1:nf

        for i=k+1:k+MAXSAT
            if rtk.x(i)==0||~test_sys(rtk.sat(i-k).sys,m)||...
                    ~rtk.sat(i-k).vsat(f)||~rtk.sat(i-k).half(f)
                continue;
            end
            if rtk.sat(i-k).lock(f)>0&&~bitand(rtk.sat(i-k).slip(f),2)&&...
                    rtk.sat(i-k).azel(2)>=rtk.opt.elmaskar&&~nofix
                rtk.sat(i-k).fix(f)=2;
                break;
            else
                rtk.sat(i-k).fix(f)=1;
            end
        end

        for j=k+1:k+MAXSAT
            if i==j||rtk.x(j)==0||~test_sys(rtk.sat(j-k).sys,m)||...
                    ~rtk.sat(j-k).vsat(f)
                continue;
            end
            if rtk.sat(j-k).lock(f)>0&&~bitand(rtk.sat(j-k).slip(f),2)&&...
                    rtk.sat(i-k).vsat(f)&&...
                    rtk.sat(j-k).azel(2)>=rtk.opt.elmaskar&&~nofix
                D(na+nb+1,i)= 1;
                D(na+nb+1,j)=-1;
                nb=nb+1;
                rtk.sat(j-k).fix(f)=2;
            else
                rtk.sat(j-k).fix(f)=1;
            end
        end

        k=k+MAXSAT;
    end

end

return


function [Gamma,Xi,L] = fb_Gamma_inference_ehmm(XX,ehmm,residuals,Gamma,Xi,T)
% Inference using forward backward propagation for parallel ehmm chains

% Note that this is an approximation, so there can be small increments of
% the free energy in this step. This is because of two reasons:
% 1. The likelihood for the free energy and for the FB equations is
% computed slightly differently: the term NormWishtrace (related to the
% W covariance) is computed across states for the FB equations, and per
% state in the free energy. That's due to the fact that the FB equations
% are approximate here.
% 2. The weights of how each chain contributes to each time point are *not*
% a linear combination of the Gamma state probabilities, because the way the weight
% for the baseline state is computed is nonlinear (and therefore everything
% is not linear). In fact, the likelihood is also a nonlinear function of
% the state probabilities. For example, we could have the highest
% likelihood for gamma_kt = 0.3, but the forward-backward equations assume
% a linear progression between 0 and 1. A potential solution would be to
% bin the Gammas, and have L states per chain: 0% active, 20% active, etc.,
% but that complicates the definition of the trans prob matrices and their
% priors.
%
% Author: Diego Vidaurre, University of Oxford / Aarhus University (2022)

order = ehmm.train.maxorder; K = ehmm.K; show_warn = true;

if nargin<6, T = size(residuals,1) + order; ttrial = T(1); N = 1;
else, ttrial = T(1); N = length(T);
end % normally this is called for a continuous segment
% if nargin<5 || isempty(Xi), Xi = zeros(ttrial-1-order,K,4); check_improv = false;
% else, check_improv = true;
% end
check_improv = false;

if ehmm.train.acrosstrial_constrained
    Gamma = squeeze(mean(reshape(Gamma,[ttrial-order,N,K]),2));
end

if check_improv
    n_rejected = 0;
    if strcmpi(ehmm.train.stopcriterion,'LogLik')
        c = sum(evalfreeenergy_ehmm(T,Gamma,Xi,ehmm,residuals,XX,[0 1 1 0 0]));
    else
        c = sum(evalfreeenergy_ehmm(T,Gamma,Xi,ehmm,residuals,XX,[1 1 1 0 0]));
    end
    %disp(num2str(loglik))
end

if isempty(Xi), Xi = zeros(ttrial-order-1,K,4); end

for k = randperm(K)

    Lk = zeros(ttrial * N, 2); % N > 1 only if we are averaging across trials

    for j = 1:N % N is 1 except when acrosstrial_constrained
        ind = (1:ttrial) + ttrial * (j-1);
        ind2 = (1:(ttrial-order)) + (ttrial-order) * (j-1);
        Lk(ind,:) = obslike_ehmm(ehmm,Gamma,residuals(ind2,:),XX(ind2,:),k);
    end

    if ehmm.train.acrosstrial_constrained
       Lk = squeeze(sum(reshape(Lk,[ttrial,N,2]),2));
    end

    scale = zeros(ttrial,1);
    alpha = zeros(ttrial,2);
    beta = zeros(ttrial,2);

    alpha(1+order,:) = ehmm.state(k).Pi.*Lk(1+order,:);
    scale(1+order) = sum(alpha(1+order,:));
    alpha(1+order,:) = alpha(1+order,:)/scale(1+order);
    for i = 2+order:ttrial
        alpha(i,:) = (alpha(i-1,:)*ehmm.state(k).P).*Lk(i,:);
        scale(i) = sum(alpha(i,:));		% P(X_i | X_1 ... X_{i-1})
        alpha(i,:) = alpha(i,:)/scale(i);
    end

    scale(scale<realmin) = realmin;

    beta(ttrial,:) = ones(1,2)/scale(ttrial);
    for i = ttrial-1:-1:1+order
        beta(i,:) = (beta(i+1,:).*Lk(i+1,:))*(ehmm.state(k).P')/scale(i);
        beta(i,beta(i,:)>realmax) = realmax;
    end

    if any(isnan(beta(:)))
        warning(['State time courses became NaN (out of precision). ' ...
            'There are probably extreme events in the data. ' ...
            'Using an approximation..' ])
        Gammak = alpha; out_precision = true;
    else
        Gammak = (alpha.*beta); out_precision = false;
    end

    Gammak = rdiv(Gammak,sum(Gammak,2));
    if check_improv, Gammak0 = Gamma(:,k); Xi0k = Xi(:,k,:); end
    Gamma(:,k) = Gammak(order+1:ttrial,1);

    if isempty(Xi), Xi = zeros(ttrial-order-1,K,4); end
    if out_precision
        xi = approximateXi(Gammak(order+1:ttrial,:),size(Gammak,1),ehmm);
        Xi(:,k,:) = reshape(xi,[size(xi,1) size(xi,2)*size(xi,3)]);
        if show_warn
            show_warn = false;
            warning('out of precision when computing xi')
        end
    else
        for i = 1+order:ttrial-1
            t = ehmm.state(k).P .* ( alpha(i,:)' * (beta(i+1,:).*Lk(i+1,:)));
            Xi(i-order,k,:) = t(:)'/sum(t(:));
        end
    end

    if check_improv
        if strcmpi(ehmm.train.stopcriterion,'LogLik')
            c2 = sum(evalfreeenergy_ehmm(T,Gamma,reshape(Xi,T-order-1,K,2,2),...
                ehmm,residuals,XX,[0 1 1 0 0]));
        else
            c2 = sum(evalfreeenergy_ehmm(T,Gamma,reshape(Xi,T-order-1,K,2,2),...
                ehmm,residuals,XX,[1 1 1 0 0]));
        end
        %disp(num2str(loglik2))
        if c2 > c % decrement of fitness, undo
            Gamma(:,k) = Gammak0; Xi(:,k,:) = Xi0k;
            n_rejected = n_rejected + 1;
        else
            c = c2;
        end
    end

end

if check_improv, disp(['%Change rejected: ' num2str(n_rejected/K) ]); end

L = scale; % only the last one

end


function msfun_realtime_elapsed(block)
% Help for Writing Level-2 M-File S-Functions:
%   web([docroot '/toolbox/simulink/sfg/f7-67622.html']
%   http://www.mathworks.com/access/helpdesk/help/toolbox/simulink/sfg/f7-67622.html

% Copyright 2009, The MathWorks, Inc.

% instance variables
myRealTimeBaseline = 0;

setup(block);

%% ---------------------------------------------------
    function setup(block)
        % Register the number of ports.
        block.NumInputPorts  = 0;
        block.NumOutputPorts = 1;

        block.SetPreCompOutPortInfoToDynamic;
        block.OutputPort(1).Dimensions  = 1;
        block.OutputPort(1).SamplingMode = 'sample';

        % Set up the states
        block.NumContStates = 0;
        block.NumDworks = 0;

        % Register the parameters.
        block.NumDialogPrms     = 0; % scale factor

        % Block is fixed in minor time step, i.e., it is only executed on major
        % time steps. With a fixed-step solver, the block runs at the fastest
        % discrete rate.
        block.SampleTimes = [0 1];

        block.SetAccelRunOnTLC(false); % run block in interpreted mode even w/ Acceleration

        % methods called at run-time
        block.RegBlockMethod('Start', @Start);
        block.RegBlockMethod('Outputs', @Output);
        block.RegBlockMethod('SimStatusChange', @SimStatusChange);
    end

%%
    function Start(block)  %#ok<INUSD>
        myRealTimeBaseline = tic;
    end

%%
    function Output(block)
         block.OutputPort(1).Data = toc(myRealTimeBaseline);
    end

%%
    function SimStatusChange(block, status)         %#ok<INUSL>
        if status == 1,  % resume
            myRealTimeBaseline = tic;
        end
    end

end


function [bndinfo2, err] = updateBoundaryInfo(bndinfo, result, im)

if isstruct(result) && isfield(result, 'regions')
    regions = result.regions;

    empty = false(numel(regions), 1);
    splab = zeros(bndinfo.nseg, 1);
    for k = 1:numel(regions)
        splab(regions{k}) = k;
        empty(k) = isempty(regions{k});
    end
    regions(empty) = [];
else
    splab = result;
    regions = cell(max(splab), 1);
    empty = false(numel(regions), 1);
    for k = 1:numel(regions)
        regions{k} = find(splab==k);
        empty(k) = isempty(regions{k});
    end
    regions(empty) = [];
end

wseg = splab(bndinfo.wseg);

% XXX only reading image until seg2fragments gets fixed
%im = imread(['/usr1/projects/dhoiem/iccv07/iccvGroundTruth/images/' bndinfo.imname]);
%im = im2double(im);

[edges, juncts, neighbors, wseg] = seg2fragments(wseg, im, 1);
bndinfo2 = processBoundaryInfo(wseg, edges, neighbors);

if isfield(bndinfo, 'imname')
    bndinfo2.imname = bndinfo.imname;
end

%bndinfo2.imname = bndinfo.imname;

stats = regionprops(bndinfo.wseg, 'Area');
area = cat(1, stats(:).Area);
bndinfo2.spArea = area;

if isfield(bndinfo, 'type')
    bndinfo2.type = bndinfo.type;
    bndinfo2.names = bndinfo.names;
    [bndinfo2.labels, err] = iccvTransferLabels(bndinfo.labels, regions, area);
    bndinfo2 = processGtBoundaryLabels(bndinfo2);

else
    err = nan;
end

bndinfo2 = orderfields(bndinfo2);


function idx = tri_find_nonmanifold_vertex(tri, x, scimat)
% TRI_FIND_NONMANIFOLD_VERTEX  Find indices of non-manifold vertices in a
% triangular mesh
%
% IDX = tri_find_nonmanifold_vertex(TRI, RCS)
%
%   This function is a hack so that we can identify which vertices
%   meshcheckrepair() identifies as non-manifold.
%
%   TRI is a 3-column matrix. Each row represents the indices of the three
%   vertices that form a triangle. TRI as a whole represents the closed
%   surface.
%
%   RCS is a 3-column matrix. Each row represents the (row, column, slice)
%   coordinates of a vertex in the mesh.
%
%   IDX is a vector with the indices of the vertices in X that are
%   non-manifold.
%
% IDX = tri_find_nonmanifold_vertex(TRI, X, SCIMAT)
%
%   Alternatively, vertex coordinates X can be provided as real world
%   (x,y,z) Cartesian coordinates. SCIMAT is a struct with the offset,
%   voxel size and orientation information (see "help scimat" for details).
%
% See also: scimat_world2index, scimat_index2world.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2013-2014 University of Oxford
% Version: 0.2.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see
% <http://www.gnu.org/licenses/>.

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

% defaults
if (nargin < 3 || isempty(scimat))
    scimat = scimat_im2scimat(zeros(2), [1 1 1], [0 0 0]);
end

% we convert the vertex coordinates to indices to avoid loss of precision
% when meshcheckrepair() writes to and reads from OFF file
rcs = round(scimat_world2index(x, scimat));

% meshcheckrepair() is supposed to be used to repair a non-manifold mesh.
% What it actually does is that it duplicates non-manifold vertices, which
% I'm not sure it's a good repair. Anyway, here we are interested only in
% detecting the non-manifold vertices. We are going to do it in a bit of a
% convoluted way, because meshcheckrepair() just duplicated the vertices,
% but doesn't tell which ones they are

% run meshcheckrepair(). This will duplicate any non-manifold vertices We
% use evalc so that we can supress the output to the screen.
[~, rcs2] = evalc('meshcheckrepair(rcs, tri, ''deep'');');

% round values to avoid finite precision errors
rcs2 = round(rcs2);

% assign one index value to each unique vertex
[~, idx] = unique(rcs2, 'rows');

% delete all vertices except for the duplicates
rcs2(idx, :) = [];

% sometimes a vertex may be repetead more than once. We want a list of
% unique vertices
rcs2 = unique(rcs2, 'rows');

% indices of non-manifold vertices
idx = find(ismember(rcs, rcs2, 'rows'));


function Dist = par_alldist(x1,x2,varargin)
% a parallelized version of vl_alldist2

opts.measure = 'L2';
opts.maxParts = 100;
opts.maxMem = [];
opts.numWorkers = 20;
opts.verbose = true;
opts = vl_argparse(opts,varargin);

if isempty(x2) || isequal(x1,x2),
    if isempty(x2), x2 = x1; end
    withSelf = true;
else
    withSelf = false;
end

if opts.numWorkers<=1,
    Dist = vl_alldist(x1,x2,opts.measure);
    return;
end

typeX1 = class(x1); typeX2 = class(x2);
if ~strcmp(typeX1,typeX2),
    error('Wrong input: x1 and x2 should be of same data type');
end

[D1, n1] = size(x1); [D2, n2] = size(x2);
if D1~=D2,
    error('Wrong input: x1 and x2 don''t match in 1st dimention');
end;
D = D1;

% see how much memory is available
tmp = x1(1);
info = whos('tmp');
numbytes = info.bytes;
if ~ispc
    [~,w] = unix('free | grep Mem');
    stats = str2double(regexp(w, '[0-9]*', 'match'));
    freemem = (stats(3)+stats(end))*1e3;
else
    stats = memory;
    freemem = stats.MemAvailableAllArrays;
end
if isempty(opts.maxMem),
    maxmem = freemem * 0.8;
else
    maxmem = min(opts.maxMem,freemem);
end
maxNum = maxmem / numbytes;

% decide window width w.r.t. constraints
w = ceil(max(sqrt(n1*n2/opts.maxParts),2*n1*n2*D/(maxNum-2*n1*n2)));
npar = ceil([n1 n2]/w);
while npar(1)*npar(2)>opts.maxParts,
    npar = max(1,npar - 1);
    w = ceil(max([n1 n2]./npar));
    npar = ceil([n1 n2]/w);
end
sz1 = [ones(1,npar(1)-1)*w n1-(npar(1)-1)*w];
sz2 = [ones(1,npar(2)-1)*w n2-(npar(2)-1)*w];

% partition data
t = npar(1)*npar(2);
x1cell = cell(1,t);
x2cell = cell(1,t);
distcell = cell(1,t);
if opts.verbose,
    ts = tic;
    fprintf('[1/3] Partitioning into %dx ((%d,%d),(%d,%d)) parts \n', ...
        t, D,w,D,w);
end
for i = 1:t,
    i1 = mod(i-1,npar(1))+1;
    i2 = floor((i-1)/npar(1))+1;
    if withSelf && i2>i1, % skip top-right triangle for self-dist
        x1cell{i} = [];
        x2cell{i} = [];
    else
        x1cell{i} = x1(:,(i1-1)*w+(1:sz1(i1)));
        x2cell{i} = x2(:,(i2-1)*w+(1:sz2(i2)));
    end
    if opts.verbose,
        if mod(i,10)==0, fprintf('.');end;
        if mod(i,200)==0, fprintf(' [%d/%d]\n',i,t); end;
    end
end
if opts.verbose, fprintf(' done! (%s)\n', timestr(toc(ts))); end;

% estimate speed
tmp = rand(1000,1000);
tt=tic;vl_alldist2(tmp,tmp,opts.measure);tc=toc(tt);
if withSelf,
    estTime = (D*(n1*n2/2-n1/2)/1e9)*tc;
else
    estTime = (D*n1*n2/1e9)*tc;
end

% real work
%{-
% comment this block for ealier MATLAB versions
pool = gcp('nocreate');
if isempty(pool) || pool.NumWorkers<opts.numWorkers,
    if ~isempty(pool), delete(pool); end
    pool = parpool(opts.numWorkers);
end
if opts.verbose,
    ts = tic;
    fprintf('[2/3] Computing distances using %d workers (~%s) ...', ...
    pool.NumWorkers, timestr(estTime/pool.NumWorkers));
end
%}
fprintf('\n');
parfor_progress(t);
parfor i=1:t,
    distcell{i} = vl_alldist2(x1cell{i},x2cell{i},opts.measure);
    parfor_progress();
end
parfor_progress(0);
if opts.verbose, fprintf(' done! (%s)\n', timestr(toc(ts))); end;

% assemble results
if opts.verbose,
    ts = tic;
    fprintf('[3/3] Assembling distance matrix \n');
end;
Dist = zeros(n1,n2,typeX1);
for i = 1:t,
    i1 = mod(i-1,npar(1))+1;
    i2 = floor((i-1)/npar(1))+1;
    if withSelf && i2>i1,
        dist_block = distcell{(i1-1)*npar(1)+i2}';
    else
        dist_block = distcell{i};
    end
    Dist((i1-1)*w+(1:sz1(i1)),(i2-1)*w+(1:sz2(i2))) = dist_block;
    if opts.verbose,
        if mod(i,10)==0, fprintf('.'); end
        if mod(i,200)==0, fprintf(' [%d/%d]\n',i,t); end;
    end
end
if opts.verbose, fprintf(' done! (%s)\n', timestr(toc(ts))); end;

end

function str=timestr(nsecs)

muls = [60 60];
% label = {'H','M','S'};
label = {'','',''};
tvec = [nsecs];

for i=1:numel(muls)
    tvec = [floor(tvec(1)/muls(end-i+1)) tvec];
    tvec(2) = mod(tvec(2),muls(end-i+1));
end

if tvec(end)~=floor(tvec(end))
    str = sprintf('%02.2f%s',tvec(end),label{end});
else
    str = sprintf('%02d%s',tvec(end),label{end});
end
for i=2:numel(tvec),
    str = sprintf('%02d%s:%s',tvec(end-i+1),label{end-i+1},str);
end

end


function [features, labels, labelprobs, weights] = ...
    mcmcGenerateGoodSegments(im, imsegs, vclassifierSP, hclassifierSP,eclassifier, ...
    segclassifier, spdata, adjlist, edata)
% Computes the marginals of the geometry for the given input image
% spdata, adjlist, edata are optional inputs

niter = 2500;

features = zeros(imsegs.nseg*6, 102);
labels = zeros(imsegs.nseg*6, 1);
labelprobs = zeros(imsegs.nseg*6, 7);
weights = zeros(imsegs.nseg*6, 1);

count = 0;

[pvSP, phSP, pE, smap] = mcmcInitialize(spdata, edata, ...
    adjlist, imsegs, vclassifierSP, hclassifierSP, eclassifier, 'labels');

imdata = mcmcComputeImageData(im, imsegs);
imdata.pvSP = pvSP;
imdata.phSP = phSP;

nseg = max(smap);

segProb = computeSegProb(segclassifier, pvSP, phSP, pE, adjlist, smap, 1:nseg);

[features, labels, labelprobs, weights, count] = ...
    addFeatures(features, labels, labelprobs, weights, count, ...
    imsegs, spdata, imdata, smap);

% do mcmc
for iter = 2:niter

    [smap, newseg, origseg, segadjmat] = mcmcGenerateProposals(pE, adjlist, smap);
   % disp(num2str(iter))

    nseg2 = max(smap);

    tmpSegProb = segProb;
    if nseg2 > nseg
        tmpSegProb(newseg) = computeSegProb(segclassifier, pvSP, phSP, pE, adjlist, smap, newseg);
        if origseg~=newseg
            tmpSegProb(origseg) = computeSegProb(segclassifier, pvSP, phSP, pE, adjlist, smap, origseg);
        end
    end

    % get all possible segmentation labelings for newseg
    neighborseg = [newseg find(segadjmat(newseg, :))];
    nn = numel(neighborseg);

    % compute transition probability to each neighbor
    transprob = zeros(nn, 1);

    sind = find(smap==newseg);
    adj1 = find((smap(adjlist(:, 1))==newseg) | (smap(adjlist(:, 2))==newseg));
    for ni = 1:nn

        s = neighborseg(ni);

        if s==newseg
            transprob(ni) = 1;
        else
            eind = adj1(find((smap(adjlist(adj1, 1))==s) | (smap(adjlist(adj1, 2))==s)));
            smap2 = smap;
            smap2(sind) = s;

            sind2 = find(smap2 > newseg);
            smap2(sind2) = smap2(sind2) - 1;
            s2 = s - (s > newseg);

            probni = computeSegProb(segclassifier, pvSP, phSP, pE, adjlist, smap2, s2);
            transprob(ni) = probni / tmpSegProb(newseg) / tmpSegProb(s);
            transprob(ni) = transprob(ni)*prod(1-pE(eind));
        end

    end

    %disp(num2str(transprob'))

    % randomly select neighbor according to transprob
    transprob = cumsum(transprob / sum(transprob));
    s = neighborseg(find(rand(1) < transprob));
    s = s(1);

    sind = find(smap==newseg);
    segProb = tmpSegProb;
    if origseg==newseg && s==newseg  % nothing changed
        % do nothing
    elseif origseg~=newseg && s==newseg  % newseg split from origseg
    else % s~=newseg - newseg merged into s

        smap(sind) = s;
        sind2 = find(smap > newseg);
        smap(sind2) = smap(sind2) - 1;
        s = s - (s > newseg);

        segProb(1:end-1) = segProb([1:newseg-1 newseg+1:end]);
        segProb(s) = computeSegProb(segclassifier, pvSP, phSP, pE, adjlist, smap, s);

    end

    if mod(iter, 500)==0
        disp(num2str(iter))
        [features, labels, labelprobs, weights, count] = ...
            addFeatures(features, labels, labelprobs, weights, count, ...
            imsegs, spdata, imdata, smap);
    end

end

features = features(1:count, :);
labels = labels(1:count);
labelprobs = labelprobs(1:count, :);
weights = weights(1:count);

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

function segProb = computeSegProb(segclassifier, pvSP, phSP, pE, adjlist, smap, sind)

nseg = numel(sind);
segProb = zeros(nseg, 1);

for i = 1:nseg

    s = sind(i);

    data = mcmcGetSegmentationFeatures(pvSP, phSP, pE, adjlist, smap, s);
    conf = test_boosted_dt_mc(segclassifier, data);
    segProb(i) = 1 / (1+exp(-conf));
end

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

function [features, labels, labelprobs, weights, count] = ...
    addFeatures(features, labels, labelprobs, weights, count, ...
    imsegs, spdata, imdata, smap)

nseg = max(smap);

for s = 1:nseg

    features(count+s, :) = mcmcGetSegmentFeatures(imsegs, spdata, imdata, smap, s);

    spind = find(smap==s);
    labelcount = zeros(1, 7);
    for sp = spind
        if imsegs.labels(sp)~=0
            labelcount(imsegs.labels(sp)) = ...
                labelcount(imsegs.labels(sp)) + imsegs.npixels(sp);
        end
    end
    labels(count+s) = 0;
    if sum(labelcount)>0
        labelprobs(count+s, :) = labelcount / sum(labelcount);
        [maxval, maxind] = max(labelprobs(count+s, :));
        if maxval >= 0.99
            labels(count+s) = maxind;
        end
    else
        labelprobs(count+s, :) = 0;
    end
    weights(count+s) = features(count+s, 54);

end

count = count + nseg;

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

function pixind = getPixels(pixlist, smap, npixels, s)

ind = find(smap==s);
npix = sum(npixels(ind));
pixind = zeros(npix, 1);

pixlist = pixlist(ind);

c = 0;
for k = 1:numel(pixlist)
    nkpix = numel(pixlist{k});
    pixind(c+1:c+nkpix) = pixlist{k};
    c = c + nkpix;
end


function varargout = model(x,parameters,nvp)
% model   A BERT model forward pass.
%
%   Z = model(X,parameters) performs inference with a BERT model on the
%   input X. X is a 1-by-numInputTokens-by-numObs array of encoded tokens.
%   The return is an array Z of size
%   (NumHeads*HeadSize)-by-numInputTokens-by-numObs. Each
%   Z(:,i,j) corresponds to the BERT embedding of input token X(1,i,j).
%
%   Z = model(X,parameters,'PARAM1', VAL1, 'PARAM2', VAL2, ...) specifies
%   the optional parameter name/value pairs:
%
%      'InputMask'             - A logical mask with the same size as X.
%                                The mask should be false at indices
%                                (i,j,k) for which X(i,j,k) corresponds to
%                                padding, and true elsewhere. The default
%                                is empty, for which the padding is
%                                inferred by the entries of X that match
%                                the PaddingCode name-value pair.
%
%      'DropoutProb'           - The probability of dropout for the output
%                                activation. It is standard to set this to a
%                                non-zero value during training, for example
%                                0.1. The default is 0.
%
%      'AttentionDropoutProb'  - The probability of dropout used in the
%                                attention layer. It is standard to set
%                                this to a non-zero value during training,
%                                for example 0.1. The default is 0.
%
%      'Outputs'               - Specify the indices of the layers to
%                                return outputs from as a vector of
%                                positive integers, or 'last' to specify
%                                the final encoder layer only. The default
%                                is 'last'.
%
%      'SeparatorCode'         - The positive integer corresponding to the
%                                separator token. The default is 103, as
%                                specified in the default BERT vocab.txt.
%
%      'PaddingCode'           - The positive integer corresponding to the
%                                padding token. The default is 1, as
%                                specified in the default BERT vocab.txt.
%
% References:
% [1] https://arxiv.org/abs/1810.04805
% [2] https://github.com/google-research/bert/

% Copyright 2021 The MathWorks, Inc.

arguments
    x dlarray {mustBeNumericDlarray,mustBeNonempty}
    parameters {mustBeA(parameters,'struct')}
    nvp.InputMask {mustBeNumericOrLogical} = logical.empty()
    nvp.DropoutProb (1,1) {mustBeNonnegative,mustBeLessThanOrEqual(nvp.DropoutProb,1),mustBeNumeric} = 0
    nvp.AttentionDropoutProb (1,1) {mustBeNonnegative,mustBeLessThanOrEqual(nvp.AttentionDropoutProb,1),mustBeNumeric} = 0
    nvp.Outputs {mustBePositive,mustBeLessThanOrEqualNumLayers(nvp.Outputs,parameters),mustBeInteger,mustBeNumeric} = parameters.Hyperparameters.NumLayers
    nvp.PaddingCode (1,1) {mustBePositive,mustBeInteger,mustBeNumeric} = 1
    nvp.SeparatorCode (1,1) {mustBePositive,mustBeInteger,mustBeNumeric} = 103
end

nvp.Outputs = nvp.Outputs(:);
nargoutchk(0,numel(nvp.Outputs));

maxLayer = max(nvp.Outputs);

w = parameters.Weights;
hyperparameters = parameters.Hyperparameters;

% Identify padding tokens - if InputMask is set just use it.
if isempty(nvp.InputMask)
    inputMask = x~=nvp.PaddingCode;
else
    assert(isequal(size(nvp.InputMask),size(x)),"bert:model:InvalidMaskSize","Expected InputMask to have same size as input X.");
    inputMask = logical(nvp.InputMask);
end

% Assuming CTB format of x.
xsz = size(x);
seq_len = xsz(2);

% Apply embeddings
types = dlarray(bert.internal.inferTypeID(x,nvp.SeparatorCode));
positions = dlarray(1:seq_len);
z = bert.layer.embedding(x,types,positions,w.embeddings,nvp.DropoutProb);

% Transformer layers
num_layers = min(hyperparameters.NumLayers,maxLayer);
varargout = cell(numel(nvp.Outputs),1);
for i = 1:num_layers
    z = bert.layer.block(z,w.encoder_layers.("layer_"+i),hyperparameters,'InputMask',inputMask);
    toAssign = nvp.Outputs==i;
    varargout(toAssign) = repelem({z},sum(toAssign));
end
end

function mustBeLessThanOrEqualNumLayers(x,params)
mustBeLessThanOrEqual(x,params.Hyperparameters.NumLayers);
end

function mustBeALogicalOrDlarrayLogical(val)
if isa(val,'dlarray')
    val = extractdata(val);
end
mustBeA(val,'logical');
end

function mustBeNumericDlarray(val)
mustBeA(val,'dlarray');
mustBeNumeric(extractdata(val));
end


%%**********************************************************************
%% blktrace: compute <X1,Z1> + ... + <Xp,Zp>
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%**********************************************************************

  function trXZ = blktrace(blk,X,Z,parbarrier);

  if (nargin == 3)
     trXZ = 0;
     for p = 1:size(blk,1)
        pblk = blk(p,:);
        if strcmp(pblk{1},'s')
           if (length(pblk{2}) == 1)
              trXZ = trXZ + sum(sum(X{p}.*Z{p}));
           else
              xx = mexsvec(pblk,X{p},0);
              zz = mexsvec(pblk,Z{p});
              trXZ = trXZ + xx'*zz;
           end
        else
           trXZ = trXZ + sum(X{p}.*Z{p});
        end
     end
  elseif (nargin == 4)
     trXZ = 0;
     for p = 1:size(blk,1)
        pblk = blk(p,:);
        if (norm(parbarrier{p}) == 0)
           if strcmp(pblk{1},'s')
              if (length(pblk{2}) == 1)
                 trXZ = trXZ + sum(sum(X{p}.*Z{p}));
              else
                 xx = mexsvec(pblk,X{p},0);
                 zz = mexsvec(pblk,Z{p});
                 trXZ = trXZ + xx'*zz;
              end
           else
              trXZ = trXZ + sum(X{p}.*Z{p});
           end
        else
           idx = find(parbarrier{p} == 0);
           if ~isempty(idx)
              if strcmp(pblk{1},'s')
                 sumXZ = sum(X{p}.*Z{p});
                 ss = [0,cumsum(pblk{2})];
                 for k = 1:length(idx)
                    idxtmp = [ss(idx(k))+1:ss(idx(k)+1)];
                    trXZ = trXZ + sum(sumXZ(idxtmp));
                 end
              elseif strcmp(pblk{1},'q')
	         tmp = qops(pblk,X{p},Z{p},1);
                 trXZ = trXZ + sum(tmp(idx));
              elseif strcmp(pblk{1},'l')
                 trXZ = trXZ + sum(X{p}(idx).*Z{p}(idx));
              end
           end
        end
     end
  end
%%**********************************************************************


clear all
uri = 'ip:192.168.86.35';

%% Turn on DDS
tx = adi.AD9361.Tx('uri',uri);
tx.DataSource = 'DDS';
tx.SamplingRate = 20e6;
tx.RFBandwidth = 20e6;
toneFreq = 4e6;
tx.DDSFrequencies = repmat(toneFreq,2,4);
tx.AttenuationChannel0 = -10;
tx();
pause(1);
%% Set up fastlock profiles
freqs = 2.4e9+(1:8).*1e6;
for f = 0:length(freqs)-1
    % Update LO
    tx.CenterFrequency = freqs(f+1);
    pause(1);
    % Save profile
    tx.setAttributeLongLong('altvoltage1','fastlock_store',int64(f),true,8);
    fprintf('Saving profile at LO %d\n',freqs(f+1));
end
% Set pin control mode
tx.setDebugAttributeBool('adi,tx-fastlock-pincontrol-enable',1);
tx.setAttributeLongLong('altvoltage1','fastlock_recall',int64(0),true,8);
%% Configure Hop mode
h = FrequencyHopper;
h.uri = uri;
h.DwellSamples = 20000;
h.HoppingEnable = true;
h();
%% Capture
rx = adi.AD9361.Rx('uri',uri);
rx.SamplingRate = 20e6;
rx.CenterFrequency = 2.4e9+0.5e6;
rx.RFBandwidth = 20e6;
rx.SamplesPerFrame = 1e6;
rx.channelCount = 2;
rx.kernelBuffersCount = 1;
for k=1:10
    valid = false;
    while ~valid
        [out, valid] = rx();
    end
end
rx.release();
%% Plot
spectrogram(double(out),128,80,100,20e6,'yaxis','centered')
view(-77,72)
shading interp
colorbar off


clear
close all
clc;

addpath(genpath('../libs'))
path_to_matconvnet = '/home/skong2/scratch/matconvnet-1.0-beta23_modifiedDagnn';

run(fullfile(path_to_matconvnet, 'matlab', 'vl_setupnn'));
addpath(genpath(fullfile('dependencies', 'matconvnet','examples')));
%% read matconvnet model
load('imdb_toydata_v3_from_mnist.mat');
imdb.path = './toydata_v3';
imdb.path_to_dataset = './toydata_v3';
% set GPU
gpuId = 4;
gpuDevice(gpuId);
flagSaveFig = true; % {true false} whether to store the result

saveFolder = 'main007_instSeg_v1_absEucMM';
modelName = 'softmax_net-epoch-83.mat';

netbasemodel = load( fullfile('./exp', saveFolder, modelName) );
netbasemodel = netbasemodel.net;

netbasemodel.layers(136).block = rmfield(netbasemodel.layers(136).block, 'ignoreAverage');
netbasemodel.layers(135).block = rmfield(netbasemodel.layers(135).block, 'ignoreAverage');

netbasemodel = dagnn.DagNN.loadobj(netbasemodel);
%% modify layers, e.g. removing
netbasemodel.meta.normalization.averageImage = imdb.meta.meanvalue;
netbasemodel.meta.normalization.imageSize = [imdb.meta.height, imdb.meta.width, 1, 1];
%% 1st mean-shift grouping loop
loopNum = 20;

sName_l2norm = 'res7_l2norm';
netbasemodel.vars(netbasemodel.layers(netbasemodel.getLayerIndex(sName_l2norm)).outputIndexes).precious = 1;

% keepLayerName = sprintf('obj_instSeg_reg');
% netbasemodel.layers(netbasemodel.getLayerIndex(keepLayerName)).block.lastLayerName = 'res7_cosSim';
% keepLayerName = sprintf('obj_instSeg_MM');
% netbasemodel.layers(netbasemodel.getLayerIndex(keepLayerName)).block.lastLayerName = 'res7_cosSim';

rmLayerName = 'obj_instSeg_MM';
if ~isnan(netbasemodel.getLayerIndex(rmLayerName))
    netbasemodel.removeLayer(rmLayerName); % remove layer
end
rmLayerName = 'obj_instSeg_reg';
if ~isnan(netbasemodel.getLayerIndex(rmLayerName))
    netbasemodel.removeLayer(rmLayerName); % remove layer
end
rmLayerName = 'res7_cosSim';
if ~isnan(netbasemodel.getLayerIndex(rmLayerName))
    netbasemodel.removeLayer(rmLayerName); % remove layer
end

gt_name =  sprintf('gt_ins');
weight_for_losses = {'obj_instSeg_reg', 1, 'obj_instSeg_MM', 1};
for loopIdx = 1:loopNum
    [netbasemodel, sName, sName_l2norm] = addOneLoop_forMeanShiftGrouping(netbasemodel, sName_l2norm, loopIdx);

    keepLayerName = sprintf('loop%d_meanshift_S_is_XX', loopIdx);
    netbasemodel.layers(netbasemodel.getLayerIndex(keepLayerName)).block.analyzeGradient = false;

    keepLayerName = sprintf('loop%d_meanshift_Y_l2norm', loopIdx);
    netbasemodel.vars(netbasemodel.layers(netbasemodel.getLayerIndex(keepLayerName)).outputIndexes).precious = 1;

    rmLayerName = sprintf('loop%d_meanshift_cosSim', loopIdx);
    netbasemodel.removeLayer(rmLayerName); % remove layer
end
%%

netbasemodel.move('gpu');
% netMat.mode = 'test';
netbasemodel.mode = 'normal';
testList = find(imdb.set==2);
rawCountsAll = {};

semanticMaskMat = [];
instanceMaskMat = [];
imgMat = [];
predInstanceMaskMat_loop0 = [];
predInstanceMaskMat = {};
for loopIdx = 1:loopNum
    predInstanceMaskMat{loopIdx} = [];
end
st = 0;
for i = 1:5%length(testList)
    curBatch = fullfile(imdb.path_to_dataset, imdb.imgList{testList(i)});
    datamat = load(curBatch);

    datamat.semanticMaskMat = reshape(datamat.semanticMaskMat, [size(datamat.semanticMaskMat,1), size(datamat.semanticMaskMat,2), 1, size(datamat.semanticMaskMat,3)]);
    datamat.instanceMaskMat = reshape(datamat.instanceMaskMat, [size(datamat.instanceMaskMat,1), size(datamat.instanceMaskMat,2), 1, size(datamat.instanceMaskMat,3)]);

    imFeed = bsxfun(@minus, datamat.imgMat, imdb.meta.meanvalue);
    inputs = {'input', gpuArray(imFeed)};
    netbasemodel.eval(inputs) ;
    res7_l2norm = gather(netbasemodel.vars(netbasemodel.layers(netbasemodel.getLayerIndex('res7_l2norm')).outputIndexes).value);
    predInstanceMaskMat_loop0(:,:,:,st+1:st+size(datamat.imgMat,4)) = res7_l2norm;

    for loopIdx = 1:loopNum
        curLayerName = sprintf('loop%d_meanshift_Y_l2norm', loopIdx);
        predInstanceMaskMat{loopIdx}(:,:,:,st+1:st+size(datamat.imgMat,4)) = ...
            gather(netbasemodel.vars(netbasemodel.layers(netbasemodel.getLayerIndex(curLayerName)).outputIndexes).value);
    end

    imgMat(:,:,:,st+1:st+size(datamat.imgMat,4)) = datamat.imgMat;
    semanticMaskMat(:,:,:,st+1:st+size(datamat.imgMat,4)) = datamat.semanticMaskMat;
    instanceMaskMat(:,:,:,st+1:st+size(datamat.imgMat,4)) = datamat.instanceMaskMat;
    fprintf('\t%d/%d\n', i, length(testList));

    st = st + size(datamat.imgMat,4);
end
%% grouping results to visualize
num = 25; % the total number of example to show
M = 5; % the number of rows in the panel, each row shows N images as below
N = 5; % the number of columns in the panel, each column shows M images as above
% h = 28;
% w = 28;
panelSZ = round(64/2)*2; % the size (height/width) of the small square image
% stepSize = (panelSZ-h)/2;

% image
showcaseSamples = stitchImage2panel(imgMat, panelSZ, M, N, 1, num);

% ground-truth semantic mask
showcaseSemanticMask= semanticMaskMat(:,:,:,1:num);
showcaseSemanticMask = reshape(showcaseSemanticMask, [numel(showcaseSemanticMask(:,:,1,1)), num]);
showcaseSemanticMask = showStitch(showcaseSemanticMask, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);

% ground-truth instance mask
showcaseInstanceMask= instanceMaskMat(:,:,:,1:num);
showcaseInstanceMask = reshape(showcaseInstanceMask, [numel(showcaseInstanceMask(:,:,1,1)), num]);
showcaseInstanceMask = showStitch(showcaseInstanceMask, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);

predInstanceMaskMat_loop0 = stitchImage2panel(predInstanceMaskMat_loop0, panelSZ, M, N, 1, num);
predInstanceMaskMat_loop0 = predInstanceMaskMat_loop0 - min(predInstanceMaskMat_loop0(:));
predInstanceMaskMat_loop0 = predInstanceMaskMat_loop0 ./ max(predInstanceMaskMat_loop0(:));

predInstanceMask = {};
for loopIdx = 1:loopNum
    predInstanceMask{loopIdx} = stitchImage2panel(predInstanceMaskMat{loopIdx}, panelSZ, M, N, 1, num);
    predInstanceMask{loopIdx} = predInstanceMask{loopIdx} - min(predInstanceMask{loopIdx}(:));
    predInstanceMask{loopIdx} = predInstanceMask{loopIdx} ./ max(predInstanceMask{loopIdx}(:));
end

%% save the results
figFolder = './figFolder';
if ~isdir(figFolder)
    mkdir(figFolder);
end
a = strfind(modelName,'epoch');
path_to_save = fullfile(figFolder, [strrep(saveFolder,'/','') '_' modelName(a(1):end-4)]);
if ~isdir(path_to_save)
    mkdir(path_to_save);
end

% summary vidualization
imgFig2 = figure(1);
subWindowH = 2;
subWindowW = 2;
set(imgFig2, 'Position', [100 100 1400 900]) % [1 1 width height]
windowID = 1;
subplot(subWindowH, subWindowW, windowID); imagesc(showcaseSamples); axis off image; title('random samples'); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(showcaseSemanticMask); axis off image; title('showcase SemanticMask'); caxis([0, 11]); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(showcaseInstanceMask); axis off image; title('showcase instanceMask'); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(predInstanceMaskMat_loop0); axis off image; title('predInstanceMask'); colorbar; windowID = windowID + 1;
if flagSaveFig
    export_fig( sprintf('%s/fig00_visualization.jpg', path_to_save) );
end

imgFig2 = figure(2);
subWindowH = 2;
subWindowW = 3;
windowID = 1;
set(imgFig2, 'Position', [100 100 1400 900]) % [1 1 width height]
for loopIdx = 1:6
    subplot(subWindowH, subWindowW, windowID);
    imagesc(predInstanceMask{loopIdx}); axis off image; title(sprintf('loop%d',loopIdx)); caxis([0, 11]); colorbar;
    windowID = windowID + 1;
end
if flagSaveFig
    export_fig( sprintf('%s/fig01_visualization_looping1.jpg', path_to_save) );
end

imgFig3 = figure(3);
subWindowH = 2;
subWindowW = 3;
windowID = 1;
set(imgFig3, 'Position', [100 100 1400 900]) % [1 1 width height]
for loopIdx = 7:12
    subplot(subWindowH, subWindowW, windowID);
    imagesc(predInstanceMask{loopIdx}); axis off image; title(sprintf('loop%d',loopIdx)); caxis([0, 11]); colorbar;
    windowID = windowID + 1;
end
if flagSaveFig
    export_fig( sprintf('%s/fig01_visualization_looping2.jpg', path_to_save) );
end

imgFig4 = figure(4);
subWindowH = 2;
subWindowW = 3;
windowID = 1;
set(imgFig4, 'Position', [100 100 1400 900]) % [1 1 width height]
for loopIdx = 13:18
    subplot(subWindowH, subWindowW, windowID);
    imagesc(predInstanceMask{loopIdx}); axis off image; title(sprintf('loop%d',loopIdx)); caxis([0, 11]); colorbar;
    windowID = windowID + 1;
end
if flagSaveFig
    export_fig( sprintf('%s/fig01_visualization_looping3.jpg', path_to_save) );
end

if flagSaveFig
    showcaseSemanticMask = uint8(showcaseSemanticMask * 255/11);
    showcaseInstanceMask = showcaseInstanceMask*255/5;

    imwrite(showcaseSamples, fullfile(path_to_save, sprintf('fig02_showcaseSamples.bmp')));
    imwrite(showcaseSemanticMask, fullfile(path_to_save, sprintf('fig03_showcaseSemanticMask.bmp')));
    imwrite(showcaseInstanceMask, fullfile(path_to_save, sprintf('fig04_showcaseInstanceMask.bmp')));
    imwrite(predInstanceMaskMat_loop0, fullfile(path_to_save, sprintf('fig05_predInstanceMask-loop0.bmp')));
    imwrite(predInstanceMask{1}, fullfile(path_to_save, sprintf('fig06_predInstanceMask-loop1.bmp')));
    imwrite(predInstanceMask{2}, fullfile(path_to_save, sprintf('fig07_predInstanceMask-loop2.bmp')));
    imwrite(predInstanceMask{3}, fullfile(path_to_save, sprintf('fig08_predInstanceMask-loop3.bmp')));
    imwrite(predInstanceMask{4}, fullfile(path_to_save, sprintf('fig09_predInstanceMask-loop4.bmp')));
    imwrite(predInstanceMask{5}, fullfile(path_to_save, sprintf('fig10_predInstanceMask-loop5.bmp')));

    save(sprintf('%s/results.mat', path_to_save), 'imgMat', 'instanceMaskMat', 'semanticMaskMat', 'predInstanceMaskMat');
end

%% leaving blank
%{
%}


%% Code to grab data, parse, adjust timing
% Adam Werries 2016, see Apache 2.0 license.
addpath('GrovesCode');
addpath('Utilities');
addpath('Tuning');

if ~exist('last_applanix_dir', 'var') || ~ischar(last_applanix_dir)
   last_applanix_dir = pwd;
end
text_files = {'*.txt;*.csv;*.log','Data files (*.txt,*.csv,*.log)'; '*.*', 'All Files (*.*)'};
%% Import Applanix %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Ground truth import
disp('Please select Applanix log file')
[applanix_file, applanix_path] = uigetfile(text_files, 'Select Applanix Log File', last_applanix_dir);
last_applanix_dir = applanix_path;

gt = csvread([applanix_path applanix_file]);
gt_t = gt(:,1);
gt_vel = gt(:,8:10);
gt_speed = gt(:,21);
gt_rot = gt(:,11:13);
gt_accel = gt(:,14:16);
gt_xyz = gt(:,27:29);

%% Import gps data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Format:  1           2         3         4         5           6     7       8
%         start_time, end_time, gps_time, fix_mode, num_sat,    lat,  long,   elev,
%         9       10       11       12       13       14       15    16    17    18    19
%         ecef_x, ecef_y,  ecef_z,  ecef_vx, ecef_vy, ecef_vz, gdop, pdop, hdop, vdop, tdop
if ~exist('last_lowcost_dir', 'var') || ~ischar(last_lowcost_dir)
   last_lowcost_dir = last_applanix_dir;
end
disp('Please select Skytraq log file')
[skytraq_file, skytraq_path] = uigetfile(text_files, 'Select Skytraq Log File', last_lowcost_dir);
last_lowcost_dir = skytraq_path;

gps = csvread([skytraq_path skytraq_file],1,0);
gps_time = gps(:,1);
% grab lat-long
lla = gps(:,6:8);
% Convert latlon to UTM
[gps_x,gps_y,utmzone] = deg2utm(lla(:,1),lla(:,2));
gps_h = -lla(:,3);

%% Import IMU data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Format:  1                2              3        4        5        6       7       8
%         meas_start_time, meas_end_time, accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z
disp('Please select IMU log file')
[imu_file, imu_path] = uigetfile(text_files, 'Select IMU Log File', last_lowcost_dir);
imu = csvread([imu_path imu_file]);
% only using end_time here
imu = imu(:,2:end);
% convert g's to m/s^2 using local gravity estimate
imu(:,2:4) = imu(:,2:4)*9.80097;
% rotate sensor readings 180 deg about the x-axis
rotation = [1   0         0;
            0   cosd(180) -sind(180)
            0   sind(180) cosd(180)];
imu(:,2:4) = (rotation*imu(:,2:4)')';
imu(:,5:7) = (rotation*imu(:,5:7)')';
%% Correct time racnges %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
timelag = -0.0963798;
[start_time,Is] = max([min(gps_time),min(gt_t),min(imu(:,1))]);
[end_time,Ie] = min([max(gps_time),max(gt_t),max(imu(:,1))]);
end_time = end_time - start_time;
gt_t = gt_t - start_time;
gps_time = gps_time - start_time + timelag;
imu(:,1) = imu(:,1) - start_time;
% cut off extra gps data at end, fold time back in
gps_mask = gps_time < end_time & gps_time > 0;
gps_time = gps_time(gps_mask);
gps = gps(gps_mask,:);
lla = lla(gps_mask,:);
gps(:,1) = gps_time;
% cut off extra IMU data at end
imu = imu(imu(:,1) < end_time, :);
imu = imu(imu(:,1) > 0, :);
imu_time = imu(:,1);

%% Generate filter time, configurable epochs
epoch = 0.04;
filter_time = 0:epoch:imu_time(end);

%% Generate ground truth

min_x = min([min(gps_x),min(gt_xyz(:,2))]);
min_y = min([min(gps_y),min(gt_xyz(:,1))]);
min_z = min([min(gps_h),min(gt_xyz(:,3))]);
gps_x = gps_x(gps_mask) - min_x;
gps_y = gps_y(gps_mask) - min_y;
gps_h = gps_h(gps_mask);
ground_truth = zeros(length(gps_time), 6);
ground_truth_full = zeros(length(filter_time), 9);

ground_truth(:,1) =  (interp1(gt_t,gt_xyz(:,2),gps_time)-min_x)';
ground_truth(:,2) =  (interp1(gt_t,gt_xyz(:,1),gps_time)-min_y)';
ground_truth(:,3) =  interp1(gt_t,gt_xyz(:,3),gps_time)';
ground_truth(:,4) =  interp1(gt_t,gt_rot(:,1),gps_time)';
ground_truth(:,5) =  interp1(gt_t,gt_rot(:,2),gps_time)';
ground_truth(:,6) =  interp1(gt_t,gt_rot(:,3),gps_time)';

ground_truth_full(:,1) = (interp1(gt_t,gt_xyz(:,2),filter_time)-min_x)';
ground_truth_full(:,2) = (interp1(gt_t,gt_xyz(:,1),filter_time)-min_y)';
ground_truth_full(:,3) = interp1(gt_t,gt_xyz(:,3),filter_time)';
ground_truth_full(:,4) = interp1(gt_t,gt_rot(:,1),filter_time)';
ground_truth_full(:,5) = interp1(gt_t,gt_rot(:,2),filter_time)';
ground_truth_full(:,6) = interp1(gt_t,gt_rot(:,3),filter_time)';
ground_truth_full(:,7) = interp1(gt_t,gt_vel(:,1),filter_time)';
ground_truth_full(:,8) = interp1(gt_t,gt_vel(:,2),filter_time)';
ground_truth_full(:,9) = interp1(gt_t,gt_vel(:,3),filter_time)';


function [pg, data, imsegs] = ijcvTestImage(im, imsegs, classifiers, smaps, spdata, adjlist, edata)
%  [pg, data, imsegs] = ijcvTestImage(im, imsegs, classifiers, smaps,
%  spdata, adjlist, edata)
%
% Computes the marginals of the geometry for the given input image
% spdata, adjlist, edata are optional inputs
% Note: only first three arguments are required

nsegments = [10 20 30 40 50 60 80 100];

% if imsegs is a cell, it contains the system command to make the
% segmentation image and the filename of the segmentation image

if iscell(imsegs)
%    syscall = imsegs{1};
%    outfn = imsegs{2};
%    system(syscall);
   segs = segment_fh(im, imsegs{1}, imsegs{2}, imsegs{3});
    imsegs = processSuperpixelImage2(segs);
end

vclassifier = classifiers.vclassifier;
hclassifier = classifiers.hclassifier;
sclassifier = classifiers.sclassifier;

if ~exist('spdata', 'var') || isempty(spdata)
    spdata = mcmcGetSuperpixelData(im, imsegs);
end

if ~exist('adjlist', 'var') || ~exist('edata', 'var') || isempty(adjlist) || isempty(edata)
    [edata, adjlist] = mcmcGetEdgeData(imsegs, spdata);
end

imdata = mcmcComputeImageData(im, imsegs);

if ~exist('smaps', 'var') || isempty(smaps)
    eclassifier = classifiers.eclassifier;
    ecal = classifiers.ecal;
    if isfield(classifiers, 'vclassifierSP')
        vclassifierSP = classifiers.vclassifierSP;
        hclassifierSP = classifiers.hclassifierSP;
        [pvSP, phSP, pE] = mcmcInitialize(spdata, edata, ...
            adjlist, imsegs, vclassifierSP, hclassifierSP, eclassifier, ecal, 'none');
    else
        pE = test_boosted_dt_mc(eclassifier, edata);
        pE = 1 ./ (1+exp(ecal(1)*pE+ecal(2)));
    end
    smaps = generateMultipleSegmentations2(pE, adjlist, imsegs.nseg, nsegments);
end

nsp = imsegs.nseg;
segs = cell(nsp, 1);

if exist('pvSP', 'var')
    for k = 1:numel(segs)
        segs{k}{end+1} = k;
    end
    pg = [pvSP(:, 1) repmat(pvSP(:, 2), 1, 5).*phSP pvSP(:, 3)];
else
    pg = zeros(nsp, 7);
end

for k = 1:size(smaps, 2)

    for s = 1:max(smaps(:, k))

        [segs, ind] = checksegs(segs, smaps(:, k), s);

        if ~isempty(ind)

            labdata = mcmcGetSegmentFeatures(imsegs, spdata, imdata, smaps(:, k), s);

            vconf = test_boosted_dt_mc(vclassifier, labdata);
            vconf = 1 ./ (1+exp(-vconf));
            vconf = vconf / sum(vconf);

            hconf = test_boosted_dt_mc(hclassifier, labdata);
            hconf = 1 ./ (1+exp(-hconf));
            hconf = hconf / sum(hconf);

            sconf = test_boosted_dt_mc(sclassifier, labdata);
            sconf = 1 ./ (1+exp(-sconf));

            pgs = [vconf(1) vconf(2)*hconf vconf(3)]*sconf;

            pg(ind, :) = pg(ind, :) + repmat(pgs, numel(ind), 1);
        end

    end

end

pg = pg ./ max(repmat(sum(pg, 2), 1, size(pg, 2)), 0.00001);

data.smaps = smaps;
data.edata = edata;
data.adjlist = adjlist;
data.spdata = spdata;
data.imdata = imdata;

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

function [segs, ind] = checksegs(segs, map, s)
% Checks whether this segment has been seen before

ind = find(map==s);

if isempty(ind)
    return;
end

% if numel(ind)==1 % already accounted for by superpixels
%     ind = [];
%     return;
% end

oldsegs = segs{ind(1)};

for k = 1:numel(oldsegs)
    if (numel(oldsegs{k})==numel(ind)) && all(oldsegs{k}==ind)
        ind = [];
        return;
    end
end

segs{ind(1)}{end+1} = ind;


function b = backT(engine, f, t)

b.t = t;
b.obslik = f.obslik;
Q = length(f.alpha);
b.beta = ones(Q,1);
b.gamma = f.alpha;
if t > 1
  bb_t = b.obslik;
  b.xi = normalise((engine.transprob .* (f.past_alpha * bb_t'))); % T-1,T
end


function [coordVERTICES,varargout] = READ_stl(stlFILENAME,varargin)
% READ_stlascii  Read mesh data in the form of an <*.stl> file
%==========================================================================
% FILENAME:          READ_stl.m
% AUTHOR:            Adam H. Aitkenhead
% INSTITUTION:       The Christie NHS Foundation Trust
% CONTACT:           adam.aitkenhead@physics.cr.man.ac.uk
% DATE:              29th March 2010
% PURPOSE:           Read mesh data in the form of an <*.stl> file.
%
% USAGE:
%
%     [coordVERTICES,coordNORMALS,stlNAME] = READ_stl(stlFILENAME,stlFORMAT)
%
% INPUT PARAMETERS:
%
%     stlFILENAME   - String - Mandatory - The filename of the STL file.
%
%     stlFORMAT     - String - Optional  - The format of the STL file:
%                                        'ascii' or 'binary'
%
% OUTPUT PARAMETERS:
%
%     coordVERTICES - Nx3x3 array - Mandatory
%                                 - An array defining the vertex positions
%                                   for each of the N facets, with:
%                                   1 row for each facet
%                                   3 cols for the x,y,z coordinates
%                                   3 pages for the three vertices
%
%     coordNORMALS  - Nx3 array   - Optional
%                                 - An array defining the normal vector for
%                                   each of the N facets, with:
%                                   1 row for each facet
%                                   3 cols for the x,y,z components of the vector
%
%     stlNAME       - String      - Optional  - The name of the STL object.
%
%==========================================================================

%==========================================================================
% VERSION:  USER:  CHANGES:
% -------   -----  --------
% 100329    AHA    Original version
% 100513    AHA    Totally reworked the code.  Now use textscan to read the
%                  file all at once, rather than one line at a time with
%                  fgetl.  Major speed improvment.
% 100623    AHA    Combined code which reads ascii STLS and code which
%                  reads binary STLs into a single function.
%==========================================================================

%==========================================================================
% STL ascii file format
%======================
% ASCII STL files have the following structure.  Technically each facet
% could be any 2D shape, but in practice only triangular facets tend to be
% used.  The present code ONLY works for meshes composed of triangular
% facets.
%
% solid object_name
% facet normal x y z
%   outer loop
%     vertex x y z
%     vertex x y z
%     vertex x y z
%   endloop
% endfacet
%
% <Repeat for all facets...>
%
% endsolid object_name
%==========================================================================

%==========================================================================
% STL binary file format
%=======================
% Binary STL files have an 84 byte header followed by 50-byte records, each
% describing a single facet of the mesh.  Technically each facet could be
% any 2D shape, but that would screw up the 50-byte-per-facet structure, so
% in practice only triangular facets are used.  The present code ONLY works
% for meshes composed of triangular facets.
%
% HEADER:
% 80 bytes:  Header text
% 4 bytes:   (int) The number of facets in the STL mesh
%
% DATA:
% 4 bytes:  (float) normal x
% 4 bytes:  (float) normal y
% 4 bytes:  (float) normal z
% 4 bytes:  (float) vertex1 x
% 4 bytes:  (float) vertex1 y
% 4 bytes:  (float) vertex1 z
% 4 bytes:  (float) vertex2 x
% 4 bytes:  (float) vertex2 y
% 4 bytes:  (float) vertex2 z
% 4 bytes:  (float) vertex3 x
% 4 bytes:  (float) vertex3 y
% 4 bytes:  (float) vertex3 z
% 2 bytes:  Padding to make the data for each facet 50-bytes in length
%   ...and repeat for next facet...
%==========================================================================

if nargin==2
  stlFORMAT = lower(varargin{1});
else
  stlFORMAT = 'auto';
end

%If necessary, identify whether the STL is ascii or binary:
if strcmp(stlFORMAT,'ascii')==0 && strcmp(stlFORMAT,'binary')==0
  stlFORMAT = IDENTIFY_stl_format(stlFILENAME);
end

%Load the STL file:
if strcmp(stlFORMAT,'ascii')
  [coordVERTICES,coordNORMALS,stlNAME] = READ_stlascii(stlFILENAME);
elseif strcmp(stlFORMAT,'binary')
  [coordVERTICES,coordNORMALS] = READ_stlbinary(stlFILENAME);
  stlNAME = 'unnamed_object';
end %if

%Prepare the output arguments
if nargout == 2
  varargout(1) = {coordNORMALS};
elseif nargout == 3
  varargout(1) = {coordNORMALS};
  varargout(2) = {stlNAME};
end

end %function

%==========================================================================
function [stlFORMAT] = IDENTIFY_stl_format(stlFILENAME)
% IDENTIFY_stl_format  Test whether an stl file is ascii or binary

% Open the file:
fidIN = fopen(stlFILENAME);

% Check the file size first, since binary files MUST have a size of 84+(50*n)
fseek(fidIN,0,1);         % Go to the end of the file
fidSIZE = ftell(fidIN);   % Check the size of the file

if rem(fidSIZE-84,50) > 0

  stlFORMAT = 'ascii';

else

  % Files with a size of 84+(50*n), might be either ascii or binary...

  % Read first 80 characters of the file.
  % For an ASCII file, the data should begin immediately (give or take a few
  % blank lines or spaces) and the first word must be 'solid'.
  % For a binary file, the first 80 characters contains the header.
  % It is bad practice to begin the header of a binary file with the word
  % 'solid', so it can be used to identify whether the file is ASCII or
  % binary.
  fseek(fidIN,0,-1);        % Go to the start of the file
  firsteighty = char(fread(fidIN,80,'uchar')');

  % Trim leading and trailing spaces:
  firsteighty = strtrim(firsteighty);

  % Take the first five remaining characters, and check if these are 'solid':
  firstfive = firsteighty(1:min(5,length(firsteighty)));

  % Double check by reading the last 80 characters of the file.
  % For an ASCII file, the data should end (give or take a few
  % blank lines or spaces) with 'endsolid <object_name>'.
  % If the last 80 characters contains the word 'endsolid' then this
  % confirms that the file is indeed ASCII.
  if strcmp(firstfive,'solid')

    fseek(fidIN,-80,1);     % Go to the end of the file minus 80 characters
    lasteighty = char(fread(fidIN,80,'uchar')');

    if findstr(lasteighty,'endsolid')
      stlFORMAT = 'ascii';
    else
      stlFORMAT = 'binary';
    end

  else
    stlFORMAT = 'binary';
  end

end

% Close the file
fclose(fidIN);

end %function
%==========================================================================

%==========================================================================
function [coordVERTICES,coordNORMALS,stlNAME] = READ_stlascii(stlFILENAME)
% READ_stlascii  Read mesh data in the form of an ascii <*.stl> file

% Read the ascii STL file
fidIN = fopen(stlFILENAME);
fidCONTENTcell = textscan(fidIN,'%s','delimiter','\n');                  %Read all the file
fidCONTENT = fidCONTENTcell{:}(logical(~strcmp(fidCONTENTcell{:},'')));  %Remove all blank lines
fclose(fidIN);

% Read the STL name
if nargout == 3
  line1 = char(fidCONTENT(1));
  if (size(line1,2) >= 7)
    stlNAME = line1(7:end);
  else
    stlNAME = 'unnamed_object';
  end
end

% Read the vector normals
if nargout >= 2
  stringNORMALS = char(fidCONTENT(logical(strncmp(fidCONTENT,'facet normal',12))));
  coordNORMALS  = str2num(stringNORMALS(:,13:end));
end

% Read the vertex coordinates
facetTOTAL       = sum(strcmp(fidCONTENT,'endfacet'));
stringVERTICES   = char(fidCONTENT(logical(strncmp(fidCONTENT,'vertex',6))));
coordVERTICESall = str2num(stringVERTICES(:,7:end));
cotemp           = zeros(3,facetTOTAL,3);
cotemp(:)        = coordVERTICESall;
coordVERTICES    = shiftdim(cotemp,1);

end %function
%==========================================================================

%==========================================================================
function [coordVERTICES,coordNORMALS] = READ_stlbinary(stlFILENAME)
% READ_stlbinary  Read mesh data in the form of an binary <*.stl> file

% Open the binary STL file
fidIN = fopen(stlFILENAME);

% Read the header
fseek(fidIN,80,-1);                   % Move to the last 4 bytes of the header
facetcount = fread(fidIN,1,'int32');  % Read the number of facets in the stl file

% Initialise arrays into which the STL data will be loaded:
coordNORMALS  = zeros(facetcount,3);
coordVERTICES = zeros(facetcount,3,3);

% Read the data for each facet:
for loopF = 1:facetcount

  tempIN = fread(fidIN,3*4,'float');

  coordNORMALS(loopF,1:3)    = tempIN(1:3);    % x,y,z components of the facet's normal vector
  coordVERTICES(loopF,1:3,1) = tempIN(4:6);    % x,y,z coordinates of vertex 1
  coordVERTICES(loopF,1:3,2) = tempIN(7:9);    % x,y,z coordinates of vertex 2
  coordVERTICES(loopF,1:3,3) = tempIN(10:12);  % x,y,z coordinates of vertex 3

  fread(fidIN,1,'int16');   % Move to the start of the next facet.  Using fread is much quicker than using fseek(fidIN,2,0);

end %for

% Close the binary STL file
fclose(fidIN);

end %function
%==========================================================================


function [cfg, movement] = ft_detect_movement(cfg, data)

% FT_SACCADE_DETECTION performs detection of movements such as saccades and
% microsaccades, but also joystick movements, from time series data over multiple
% trials. Different methods for detecting movements are implemented, which are
% described in detail below:
%
% VELOCITY2D - detects micro/saccades using a two-dimensional (2D) velocity according
% to "Engbert R, Kliegl R (2003) Vision Res 43:1035-1045". The vertical and the
% horizontal eyetracker time series (for one eye) are transformed into velocities and
% microsaccades are indentified as "outlier" eye movements that exceed a given
% threshold for velocity and duration. This method has the additional options
%     cfg.velocity2D.kernel   = vector 1 x nsamples, kernel to compute velocity (default = [1 1 0 -1 -1].*(data.fsample/6);
%     cfg.velocity2D.demean   = 'no' or 'yes', whether to apply centering correction (default = 'yes')
%     cfg.velocity2D.mindur   = minimum microsaccade durantion in samples (default = 3);
%     cfg.velocity2D.velthres = threshold for velocity outlier detection (default = 6);
%
% CLUSTERING - detects movements according to "Otero-Millan et al., (2014) J Vis 14".
%
% Use as
%   [cfg, movement] = ft_detect_movement(cfg, data)
% where the input data should be organised in a structure as obtained from the
% FT_PREPROCESSING function.
%
% The configuration can contain the following options
%   cfg.method  = string representing the method for movement detection
%                 'velocity2D' detects microsaccades using the 2D velocity
%                 'clustering' use unsupervised clustering method to detect microsaccades
%   cfg.channel = Nx1 cell-array with selection of channels, see FT_CHANNELSELECTION for details, (default = 'all')
%   cfg.trials  = 'all' or a selection given as a 1xN vector (default = 'all')
%
% The output argument "movement" is a Nx3 matrix. The first and second columns
% specify the begining and end samples of a movement period (saccade, joystick, ...),
% and the third column contains the peak velocity/acceleration movement. The thrid
% column allows to convert movements into spike data representation, making it
% compatible with the spike toolbox functions.
%
% To facilitate data-handling and distributed computing you can use
%   cfg.inputfile   =  ...
%   cfg.outputfile  =  ...
% If you specify one of these (or both) the input data will be read from a *.mat
% file on disk and/or the output data will be written to a *.mat file. These mat
% files should contain only a single variable, corresponding with the
% input/output structure.
%
% See also FT_DATABROWSER, FT_DATATYPE_SPIKE

% Copyright (C) 2014, Diego Lozano-Soldevilla
%
% 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$

% FIXME the help mentioned the

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% the initial part deals with parsing the input options and data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% these are used by the ft_preamble/ft_postamble function and scripts
ft_revision = '$Id$';
ft_nargin   = nargin;
ft_nargout  = nargout;

% do the general setup of the function
ft_defaults
ft_preamble init
ft_preamble debug
ft_preamble loadvar data
ft_preamble provenance data

% ensure that the input data is valid for this function, this will also do
% backward-compatibility conversions of old data that for example was
% read from an old *.mat file
data = ft_checkdata(data, 'datatype', {'raw'}, 'feedback', 'yes', 'hassampleinfo', 'yes');

% check if the input cfg is valid for this function
cfg = ft_checkconfig(cfg, 'forbidden',  {'channels', 'trial'}); % prevent accidental typos, see issue 1729

% set the defaults
cfg.method   = ft_getopt(cfg, 'method',   'velocity2D');
cfg.feedback = ft_getopt(cfg, 'feedback', 'yes');

if isfield(data, 'fsample')
  fsample = getsubfield(data, 'fsample');
else
  fsample = 1./(mean(diff(data.time{1})));
end

% set the defaults for the various microsaccade detection methods
switch cfg.method
  case 'velocity2D'
    % Engbert R, Kliegl R (2003) Microsaccades uncover the orientation of
    % covert attention. Vision Res 43:1035-1045.
    kernel = [1 1 0 -1 -1].*(fsample/6); % this is equivalent to Engbert et al (2003) Vis Res, eqn. (1)
    if ~isfield(cfg.velocity2D, 'kernel'),   cfg.velocity2D.kernel  = kernel; end
    if ~isfield(cfg.velocity2D, 'demean'),   cfg.velocity2D.demean  = 'yes';  end
    if ~isfield(cfg.velocity2D, 'mindur'),   cfg.velocity2D.mindur  =  3;     end % minimum microsaccade duration in samples
    if ~isfield(cfg.velocity2D, 'velthres'), cfg.velocity2D.velthres = 6;     end
  case 'clustering'
    ft_error('not implemented yet');
    % Otero-Millan J, Castro JLA, Macknik SL, Martinez-Conde S (2014)
    % Unsupervised clustering method to detect microsaccades. J Vis 14.
  otherwise
    ft_error('unsupported option for cfg.method');
end % switch method

% select channels and trials of interest, by default this will select all channels and trials
tmpcfg = keepfields(cfg, {'trials', 'channel', 'tolerance', 'showcallinfo', 'trackcallinfo', 'trackusage', 'trackdatainfo', 'trackmeminfo', 'tracktimeinfo', 'checksize'});
data = ft_selectdata(tmpcfg, data);
[cfg, data] = rollback_provenance(cfg, data);

% determine the size of the data
ntrial = length(data.trial);
nchan  = length(data.label);   % number of channels

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% the actual computation is done in the middle part
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
movement = [];

ft_progress('init', cfg.feedback, 'processing trials');
% do all the computations
for i=1:ntrial
  ft_progress(i/ntrial, 'finding microsaccades trial %d of %d\n', i, ntrial);

  dat = data.trial{i};
  nsample = size(dat,2);

  switch cfg.method
    case 'velocity2D'

      % demean horizontal and vertical time courses
      if strcmp(cfg.velocity2D.demean, 'yes')
        dat = ft_preproc_polyremoval(dat, 0, 1, nsample);
      end

      %% eye velocity computation
      % deal with padding
      n = size(cfg.velocity2D.kernel,2);
      pad = ceil(n/2);
      dat = ft_preproc_padding(dat, 'localmean', pad);

      % convolution. See Engbert et al (2003) Vis Res, eqn. (1)
      if n<100
        % heuristic: for large kernel the convolution is faster when done along
        % the columns, weighing against the costs of doing the transposition.
        % the threshold of 100 is a bit ad hoc.
        vel = convn(dat,   cfg.velocity2D.kernel,   'same');
      else
        vel = convn(dat.', cfg.velocity2D.kernel.', 'same').';
      end
      % cut the eges
      vel = ft_preproc_padding(vel, 'remove', pad);

      %% microsaccade detection
      % compute velocity thresholds as in Engbert et al (2003) Vis Res, eqn. (2)
      medianstd = sqrt( median(vel.^2,2) - (median(vel,2)).^2 );

      % Engbert et al (2003) Vis Res, eqn. (3)
      radius = cfg.velocity2D.velthres*medianstd;

      % compute test criterion: ellipse equation
      test = sum((vel./radius(:,ones(1,nsample))).^2,1);
      sacsmp = find(test>1); % microsaccade's indexing

      %% determine microsaccades per trial
      % first find eye movements of n-consecutive time points
      j = find(diff(sacsmp)==1);
      j1 = [j; j+1];
      com = intersect(j,j+1);
      cut = ~ismember(j1,com);
      sacidx = reshape(j1(cut),2,[]);

      for k=1:size(sacidx,2)
        duration = sacidx(1,k):sacidx(2,k);
        if size(duration,2) >= cfg.velocity2D.mindur
          % finding peak velocity by Pitagoras
          begtrl = sacsmp(duration(1,1));
          endtrl = sacsmp(duration(1,end));

          [peakvel, smptrl] = max(sqrt(sum(vel(:,begtrl:endtrl).^2,1)));
          veltrl = sacsmp(duration(1,smptrl)); % peak velocity microsaccade sample -> important for spike conversion

          trlsmp = data.sampleinfo(i,1):data.sampleinfo(i,2);
          begsample = trlsmp(1, begtrl); % begining microsaccade sample
          endsample = trlsmp(1, endtrl); % end microsaccade sample
          velsample = trlsmp(1, veltrl); % velocity peak microsaccade sample
          movement(end+1,:) = [begsample endsample velsample];
        end
      end

    case 'clustering'
      ft_error('not implemented yet');
      % Otero-Millan J, Castro JLA, Macknik SL, Martinez-Conde S (2014)
      % Unsupervised clustering method to detect microsaccades. J Vis 14.

    otherwise
      ft_error('unsupported option for cfg.method');
  end % switch method
end % for each trial
ft_progress('close');

% do the general cleanup and bookkeeping at the end of the function
ft_postamble provenance
ft_postamble debug
ft_postamble previous data


function  summaryWithFgVolume = dtiExtrapolateFgVolumeFromCrossectionArea(summaryWithCrossectionArea, summaryWithFiberGroupLength)
%Compute crosssection area for fiber groups/subjects
%
% summary = dti_Longitude_ComputeCrossectionalArea(summaryWithCrosSectionalArea, ...
%                                                      [summaryWithFiberGroupLength])
%
% Use case 1
% Input: a summary structure of fg properties returned by
% dtiFiberProperties
% Output:  same summary structure with field
% diberGroupVolume added or updated.
%
% Use case 2
% If two summary structures are provided as arguments, then
% the fgVolume will be extracted from the first one, and fiberLength will
% be extracted from the second one. The resulting summary will contain ONLY
% cross-sectional area estimates, and only for subjects present in both
% input summary structures. Note:it is OK if subjects order is not
% equivalent in both input summary structures. It is NOT ok if the fiber
% groups are not -- u should check that!
%
% Example:
%   load('summaryFiberPropertiesMoriGroups_Roi2RoiVolumeUniqueVoxels.mat');
%   summaryWithCrossectionArea = dtiComputeCrossectionArea(summary);
%
% See a more elaborate example after "return" statement below.
%
% See also: dtiFiberProperties, dtiComputeCrossectionalArea
%
% (c) Vistasoft

% History: ER wrote it 03/24/2010

fprintf(1, 'Computing volume of a fiber group from its crossectional area and length \n');

if nargin == 1
    summaryWithFgVolume = summaryWithCrossectionArea;
    summaryWithFiberGroupLength = summaryWithCrossectionArea;
elseif nargin == 2
    summaryWithFgVolume =[];
else
    eror ('Incorrect number of arguments');
end

for s=1:length(summaryWithCrossectionArea)

    s2 = find(arrayfun(@(x) strcmp(x.subject,summaryWithCrossectionArea(s).subject), summaryWithFiberGroupLength));

    for fg=1:numel(summaryWithCrossectionArea(s).sfg);

        summaryWithFgVolume(s).sfg(fg).fiberGroupVolume = summaryWithCrossectionArea(s).sfg(fg).crossectionArea*summaryWithFiberGroupLength(s2).sfg(fg).fiberLength(2);
        summaryWithFgVolume(s).sfg(fg).name = summaryWithCrossectionArea(s).sfg(fg).name;
        summaryWithFgVolume(s).subject = summaryWithCrossectionArea(s).subject;
    end
end

return

%% An 'elaborate' example
cd /biac3/wandell4/users/elenary/longitudinal/ANALYSES/Mori_Groups/

%load 'summary' structure with Roi2Roi cropped fiber groups' properties
load('summaryFiberPropertiesMoriGroups_Roi2RoiVolumeUniqueVoxels.mat');
%Compute crossectional areas
summaryWithCrossectionArea = dtiComputeCrossectionArea(summary);

%load 'summary' with original (full length) FG properties, incl. length
load('summaryFiberPropertiesMoriGroups_volumeUniqueVoxels.mat');
summaryWithFiberGroupLength = summary;
%Compute extrapolated volume from realLength and approximated Xarea
summaryFgVolumeExtra = dtiExtrapolateFgVolumeFromCrossectionArea(summaryWithCrossectionArea, summaryWithFiberGroupLength);

%Now your summary(1).sfg(1).fiberGroupVolume contains actual volume of fg1 for s1,
%and summaryVolumeExtra(1).sfg(1).fiberGroupVolume contains the volume
%of fg1 for s1 estimated as crossectional area of the middle (Roi to
%Roi, more compact) portion of the fg, extrapolated to the its full
%length.

%Now plot some stuff
labels = dtiGetMoriLabels;
labels_short=dtiGetMoriLabels(true);
load('../../data/subjectCodesAll4Years'); %This will load subjectCodes
subjectInitials={'ab', 'ajs', 'am', 'an', 'at', 'clr', 'crb', 'ctb', 'da', 'dh', 'dm', 'es', 'jh', 'lj', 'll', 'mb', 'md', 'mho', 'mn', 'pf', 'pt', 'rd', 'rsh', 'ss', 'tm', 'vr', 'vt', 'zs'};
[fiberMetrics_realVol, subjectCodes, year, group] = dtiGetFGProperties(summaryWithFiberGroupLength, subjectInitials, 'fiberGroupVolume', labels, '0');
[fiberMetrics_realLength, subjectCodes, year, group] = dtiGetFGProperties(summaryWithFiberGroupLength, subjectInitials, 'fiberLength', labels, '0');
[fiberMetrics_XArea, subjectCodes, year, group] = dtiGetFGProperties(summaryWithCrossectionArea, subjectInitials, 'crossectionArea', labels, '0');
[fiberMetrics_extrapVol, subjectCodes, year, group] = dtiGetFGProperties(summaryFgVolumeExtra, subjectInitials, 'fiberGroupVolume', labels, '0');
[fiberMetrics_FA, subjectCodes, year, group] = dtiGetFGProperties(summaryWithFiberGroupLength, subjectInitials, 'FA', labels, '0');

%Need to know numFibers in order to collapse data across somefiber groups.
[numFibers_fullFGs, subjectCodes, year, group] = dtiGetFGProperties(summaryWithFiberGroupLength, subjectInitials, 'numberOfFibers', labels, '0');

%Reduced 20 fiber groups to 9
%collapsingVector2 = [1 2 3 4 5 6 5 6 7 8 9 10 11 12 13 14 15 16 13 14]; %will combine cingulum cingulate and hc, and will combine slf_t and slf_fp.
collapsingVector2 = [1 1 2 2 3 3 3 3 4 5 6 6 7 7 8 8 9 9 8 8]; %will collapse symmetric fiber groups into one, will combine cingulum cingulate and hc, and will combine slf_t and slf_fp.

[fiberMetrics_realVol, labels_New] = dti_LongitudeAggregateFiberPropertiesAcrossGroups(fiberMetrics_realVol, labels_short, collapsingVector2, 'sum');
[fiberMetrics_extrapVol, labels_New] = dti_LongitudeAggregateFiberPropertiesAcrossGroups(fiberMetrics_extrapVol, labels_short, collapsingVector2, 'sum');
[fiberMetrics_realLength, labels_New] = dti_LongitudeAggregateFiberPropertiesAcrossGroups(fiberMetrics_realLength, labels_short, collapsingVector2, 'mean', numFibers_fullFGs);
[fiberMetrics_XArea, labels_New] = dti_LongitudeAggregateFiberPropertiesAcrossGroups(fiberMetrics_XArea, labels_short, collapsingVector2, 'sum');
[fiberMetrics_FA, labels_New] = dti_LongitudeAggregateFiberPropertiesAcrossGroups(fiberMetrics_FA, labels_short, collapsingVector2, 'mean', numFibers_fullFGs);

% Length -> Volume & Extrapolated volume
figure; plot(fiberMetrics_realLength(:), fiberMetrics_extrapVol(:), 'ro'); xlabel('real length'); hold on;
plot(fiberMetrics_realLength(:), fiberMetrics_realVol(:), 'bx');  legend('Extrapolated volume, mm^3', 'Real volume, mm^3');

%figure; plot(log(realVol), log(extrapolatedVol), 'ro'); xlabel('Log (real volume)'); ylabel('Log (extrapolated volume)');
f1=figure;
screen_size = get(0, 'ScreenSize'); set(f1, 'Position', [0 0 screen_size(3) screen_size(4) ] );
colordef black

markers=getUniqueMarkers(20);
for fg=1:length(labels_New)
    ax1 = subplot(2, 3, 1); hold on;
    plot(fiberMetrics_realLength(:, fg), fiberMetrics_XArea(:, fg), markers{fg}); ylabel('Crossectional area ROi2Roi, mm^2'); xlabel('real length');
    ax2 = subplot(2, 3, 2); hold on;
    plot(fiberMetrics_realVol(:, fg), fiberMetrics_extrapVol(:, fg), markers{fg}); ylabel('extrapolated volume, mm^3'); xlabel('real volume, mm^3');
    ax3 = subplot(2,3, 3); hold on;
    plot(fiberMetrics_XArea(:, fg), fiberMetrics_realVol(:, fg), markers{fg}); ylabel('Real volume'); xlabel('crossection area Roi2Roi'); h=legend(labels_New);
    ax4 = subplot(2, 3, 4); hold on;
    plot(fiberMetrics_FA(:, fg), fiberMetrics_realLength(:, fg), markers{fg}); ylabel('realLength');
    ax5 = subplot(2, 3, 5); hold on;
    plot(fiberMetrics_FA(:, fg), fiberMetrics_realVol(:, fg),  markers{fg}); ylabel('realVolume'); xlabel('FA');
    ax6 = subplot(2, 3, 6); hold on; plot(fiberMetrics_FA(:, fg), fiberMetrics_XArea(:, fg), markers{fg}); ylabel('Crossection Area'); xlabel('FA');
end
pL=get(h, 'Position');
p1 = get(ax1, 'Position');
set(h, 'Position', [p1(1) p1(4) pL(3) pL(4)]);
%colordef white


clear all;close all;clc

M_dir = {'F:\Janelia2014\Fish1_16states_30frames';
    'F:\Janelia2014\Fish2_20140714_2_4_16states_10frames';
    'F:\Janelia2014\Fish3_20140715_1_1_16_states_10frames';
    'F:\Janelia2014\Fish4_20140721_1_8_16states_20frames';
    'F:\Janelia2014\Fish5_20140722_1_2_16states_30frames';
    'F:\Janelia2014\Fish6_20140722_1_1_3states_30,40frames';
    'F:\Janelia2014\Fish7_20140722_2_3_3states_30,50frames';
    'F:\Janelia2014\Fish8_20141222_2_2_7d_PT_3OMR_shock_lowcut';
    'F:\Janelia2014\Fish9_20150120_2_1_photo_OMR_prey_blob_blue_cy74_6d_20150120_220356'};

poolobj=parpool(4);
%%
for i_fish = 9,%1:8,
    disp(['i_fish = ', num2str(i_fish)]);

    datadir = M_dir{i_fish};
    file = fullfile(datadir,['Fish' num2str(i_fish) '_direct_load.mat']);
    load(file);
    %     varList = {'CR_raw','nCells','CInfo','anat_yx','anat_yz','anat_zx','ave_stack','fpsec','frame_turn','periods'};

    %% detrend
    CR_dtr = zeros(size(CR_raw));
    tmax=size(CR_raw,2);
    ncells=size(CR_raw,1);

    tic
    parfor i=1:ncells,
        cr = CR_raw(i,:);
        crd = 0*cr;
        for j=1:100:tmax,
            if j<=150,
                tlim1 = 1;
                tlim2 = 300;
            elseif j>tmax-150,
                tlim1 = tmax-300;
                tlim2 = tmax;
            else
                tlim1 = j-150;
                tlim2 = j+150;
            end
            crr = cr(tlim1:tlim2);
            crd(max(1,j-50):min(tmax,j+50)) = prctile(crr,15);
        end
        if mod(i,100)==0,
            disp(num2str(i));
        end
        CR_dtr(i,:) = zscore(cr-crd);
    end
    toc

    CR_dtr = single(CR_dtr);

    %% save new version
    varList = {'CR_raw','CR_dtr','nCells','CInfo','anat_yx','anat_yz','anat_zx','ave_stack','fpsec','frame_turn','periods'};
    save(file,varList{:});
end

delete(poolobj);


%%
 %  Copyright (c) 2014, Facebook, Inc.
 %  All rights reserved.
 %
 %  This source code is licensed under the BSD-style license found in the
 %  LICENSE file in the root directory of this source tree. An additional grant
 %  of patent rights can be found in the PATENTS file in the same directory.
 %
 %%
function features = prepare_level1_global_features(patches, config)
dims = config.CNN_GLOBAL_MODEL_DIMS;
crop_dims = dims - 2 * config.CNN_GLOBAL_FRINGE;
N = size(patches,4);

% Change the input_dim to be 100 in the deploy.prototxt
matcaffe_init(config.USE_GPU, config.GLOBAL_DEF, config.GLOBAL_MODEL);

d = load(config.GLOBAL_MEAN_FILE);
IMAGE_MEAN = d.image_mean;

% permute from RGB to BGR (IMAGE_MEAN is already BGR)
patches = patches(:, :, [3 2 1], :);
features = zeros(N, 4096 * 2);
input = cell(1, 1);
for i = 1 : ceil(N/ 100)
    fprintf('Extract global features for batch %d\n',i);
    idx = [100 * (i - 1) + 1 : min(N, 100 * i)];
    input{1} = single(zeros(crop_dims(1), crop_dims(2), 3, 100));
    % the top half
    patches_1 = single(patches(1:dims(1),:,:,idx)) - repmat(IMAGE_MEAN, [1 1 1 length(idx)]);
    % flip width and height to make width the fastest dimension
    patches_1 = permute(patches_1, [2 1 3 4]);
    input{1}(:, :, :, 1:length(idx)) = patches_1(config.CNN_GLOBAL_FRINGE + (1:crop_dims(1)), ...
             config.CNN_GLOBAL_FRINGE + (1:crop_dims(2)), :, :);
    results = caffe('forward', input);
    fea_top = reshape(results{1}, [4096 100]);
    clear results;
    fea_top = fea_top(:, 1:length(idx));

    % the bottom half
    patches_2 = single(patches((dims(1)+1): (dims(1)*2),:,:,idx)) - repmat(IMAGE_MEAN, [1 1 1 length(idx)]);
    patches_2 = permute(patches_2, [2 1 3 4]);
    input{1}(:, :, :, 1:length(idx)) = patches_2(config.CNN_GLOBAL_FRINGE + (1:crop_dims(1)),...
        config.CNN_GLOBAL_FRINGE + (1:crop_dims(2)), :, :);
    results = caffe('forward', input);
    fea_bottom = reshape(results{1}, [4096 100]);
    fea_bottom = fea_bottom(:, 1:length(idx));
    features(idx, :)  = [fea_top' fea_bottom'];
end


% Select target active features.

% Input:
%   feature_groups    -  a cell array with each cell is one kind of
%                        features of the search image window
%   filter_sz         -  size of the filter [height wieth depth], also the
%                        target size in the feature map.
%   sel_feat_num      -  selected feature numbers

% Output:
%   feat_weight -   weights of each features.
%   IAF_model   -   initialized IAF model

% by Xin Li, April 2 2018
function [feat_weights,w_balance] = TAF_model( feature_groups, filter_sz)

assert(iscell(feature_groups),'the input feature should be a cell array!\n');
feat_num     = numel(feature_groups);
feat_weights = cell(1,feat_num);
channel_num  = [300,80]; % we select 300 and 80 filters from the conv43 and conv41 layer features.
nz_num       = 0;
nz_num_min   = 250;       % the minimum filter number.
w_balance    = gpuArray(single([1 1 1])); % weights leveraging these two features.

%peform feature selection on the conv41 and conv43 layer features.
for layer_i=feat_num:-1:1
    %% initialization
        filter_sz(3) = size(feature_groups{layer_i},3);
        [model.net] = TAF_net_init(filter_sz);

        %generate the Gaussian label map
        output_sigma = filter_sz([1:2])*0.1; % output_sigma_factor=0.1
        [h,w,~] = size(feature_groups{layer_i}); sw_sz = [h w]; % search window size [height width]
        model.label_gaussian = single(gaussian_shaped_labels(output_sigma, sw_sz));
        model.label_binary = single(binary_labels(filter_sz([1:2]), sw_sz, [0.3 0.7]));
    %% First train the network with ridge loss
        trainOpts.numEpochs=100;
        trainOpts.learningRate=5e-7;
        trainOpts.weightDecay= 1;
        trainOpts.batchSize = 1 ;
        trainOpts.gpus = 1 ;
        trainOpts.train = 1;
        input = {feature_groups{layer_i}, gpuArray(model.label_gaussian)};

        [net_trained, ~, ders_iter] = cnn_train_dag_ridge(model.net, [], input, [], trainOpts);
        [v,i] = sort(sum(sum(net_trained.params(1).value)),'descend'); % GAP.
        % The value of the parameters equals to the sum of the gradients in
        % all BP processes. And we found that using the converged parameters is more
        % stable.
    %% mapping gradients values to weights
        feat_weight=zeros(1,1,filter_sz(3), 'like', feature_groups{layer_i});
        feat_weight(i(v>0))=1;     % here we select the filters with positive gradient values, which have a positive relationship with the target loss.
        if layer_i>1 % we perform scale sensitive feature selection on the conv41 feature, as it retains more spatial information.
            filter_sz(3) = size(feature_groups{layer_i},3);
            % initialize the ranking loss network
            [IAF_net_rank, filter_sz_new] = TAF_net_init_rank(filter_sz);
            % compute the scale sensitive features
            [temp_weight, temp_i] = rank_selection(feature_groups{layer_i}, filter_sz_new, IAF_net_rank);
            % select the features not only are active to the target but also are scale-sensitieve.
            feat_weight = feat_weight.*temp_weight;
        end
        feat_weight(i(channel_num(layer_i)+1:end)) = 0; % we only select the first N (channel_num) filters.
        nz_num=nz_num+sum(feat_weight);

        % In case, there are too less features, we set a minimum feature
        % number. If the total number is less than the minimum number, then select more from conv43
        if layer_i==1 && nz_num<nz_num_min
            added_inds=(i(sum(feat_weight)+1:sum(feat_weight)+nz_num_min-nz_num));
            feat_weight(added_inds) = 1;
        end

        % To combine the features from conv41 and conv43 effective, we define
        % balance weights based on the maximum value in their features.
        feat_weights{layer_i}=feat_weight;
        w_balance(layer_i) = max(max(sum(feature_groups{layer_i}(:,:,i(1:50)),3)));
        clear net_trained;

end
        w_balance=w_balance(1)./w_balance;
end

function [feat_weight, i_index] = rank_selection(feat, filter_sz, net)
% this function selects the scale sensitve features based on the ranking
% loss.

    channel_num= size(feat,3);
    [scale_samples, pair_label, pairs, locs] = gen_ranked_samples(feat, filter_sz(1:2));

% gradients of samples with different scales
    inputs = {'input', gpuArray(scale_samples), 'scale_label', gpuArray(pair_label)};
    net.eval(inputs,{'objective_scale',1});
    feat_der = sum(net.vars(1).der,4);

    [v_scale,i_index] = sort(sum(sum(sum(feat_der)),4), 'descend');
    feat_weight=ones(1,1,channel_num, 'like', feat)*0;
    feat_weight(i_index(v_scale>0))=1;  % we select the features, which have positive relationships with the ranking loss.
end

function [IAF_net_rank,filter_sz] = TAF_net_init_rank(filter_sz)
rng('default');

channel=filter_sz(3);

rw=ceil(filter_sz(2)/2);
rh=ceil(filter_sz(1)/2);

fw=2*rw+1;
fh=2*rh+1;

filter_sz = [fh,fw,channel];

IAF_net_rank=dagnn.DagNN();

%% rank loss net

IAF_net_rank.addLayer('conv12', dagnn.Conv('size', [fw,fh,channel,1],...
    'hasBias', true, 'pad',...
    [0,0,0,0], 'stride', [1,1]), 'input', 'conv12', {'conv21_f', 'conv21_b'});

f = IAF_net_rank.getParamIndex('conv21_f') ;
IAF_net_rank.params(f).value=single(randn(fh,fw,channel,1) /...
    sqrt(rh*rw*channel))/1e8;
IAF_net_rank.params(f).learningRate=1;
IAF_net_rank.params(f).weightDecay=1e3;

f = IAF_net_rank.getParamIndex('conv21_b') ;
IAF_net_rank.params(f).value=single(zeros(1,1));
IAF_net_rank.params(f).learningRate=2;
IAF_net_rank.params(f).weightDecay=1e3;
IAF_net_rank.addLayer('rank_loss',...
    RankingLoss(),{'conv12','scale_label'},'objective_scale');

IAF_net_rank.move('gpu');
end


%{
 * Copyright (C) 2020-2030, The Regents of The University of Michigan.
 * All rights reserved.
 * This software was developed in the Biped Lab (https://www.biped.solutions/)
 * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
 * be available under alternative licensing terms; contact the address above.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * The views and conclusions contained in the software and documentation are those
 * of the authors and should not be interpreted as representing official policies,
 * either expressed or implied, of the Regents of The University of Michigan.
 *
 * AUTHOR: Bruce JK Huang (bjhuang[at]umich.edu)
 * WEBSITE: https://www.brucerobot.com/
%}

function [X_clean, bag_data] = cleanLiDARTarget(scan_num, tag_num, bag_data, X, target_len, opt)
    N = 4; % clean up using N std for x axis
    M = 3; % clean up using M std for y, z axis
    opt = optimizeCost(opt, X, target_len, 0.001);
    X_ref = opt.H_opt * X;
%     distance = sum(X_ref(1:3,:), 1); % L1
    L_infinity = max(abs(X_ref(1:3,:)));  % L infinity
    K = find(L_infinity < (target_len/2)*1.025);
    X_ref_clean_yz = X_ref(:, K); % clean up y and z axis
    L_infinity = max(X_ref_clean_yz(1:3,:));
    K_center = find(L_infinity < target_len/4);
    X_std = std(X_ref_clean_yz(:, K_center), 1, 2);
    Q = find(abs(X_ref_clean_yz(1,:)) < N*(X_std(1))); % clean up x with 2 std
    X_ref_clean = X_ref_clean_yz(:, Q);
    X_clean = inv(opt.H_opt) * X_ref_clean;
    bag_data.lidar_target(tag_num).scan(scan_num).clean_up.std = N*(X_std(1));
    bag_data.lidar_target(tag_num).scan(scan_num).clean_up.L_infinity = L_infinity;
    bag_data.lidar_target(tag_num).scan(scan_num).clean_up.L_1 = sum(X_ref(1:3,:), 1);
%     figure(200);
%     clf('reset')
%     scatter3(X_ref(1,:), X_ref(2,:), X_ref(3,:))
%     xlabel('x')
%     ylabel('y')
%     zlabel('z')
%     axis equal
%     hold on;
%     scatter3(X_ref_clean_yz(1,:), X_ref_clean_yz(2,:), X_ref_clean_yz(3,:))
%     scatter3(X_ref_clean(1,:), X_ref_clean(2,:), X_ref_clean(3,:))
%     axis equal
%     view(90,0)
%     hold off;
end