function [ readerobj ] = open_ph_reader( filename, varargin )
%OPEN_PH_READER Open a generic reader for multiple formats of phase history data
%
% Returns a reader object structure with the following methods:
%
% [wbvectors, per_pulse_metadata] = readerobj.read_cphd(pulse_indices, sample_indices, channels)
%      If file format holds raw phase history, the pulse data in the file
%      is read and then the correct processing is done so that motion
%      compensated pulses and metadata are returned.  If the file format
%      holds motion compensated phase history, this data is returned
%      directly.  Either way, this function always returns data at a
%      consistent stage in the processing chain.
%      PULSE_INDICES: Vector with indices to all desired pulses.  Can
%         also use the string 'all' to specify all pulses.  Default is all
%         pulses.
%      SAMPLES_INDICES: Vector with indices to all desired pulses.  Can
%         also use the string 'all' to specify all samples.  Default is
%         all samples.
%      CHANNELS: Vector with indices to all desired channels.  Can
%         also use the string 'all' to specify all pulses.  Default is 1.
%      WBVECTORS: The returned phase history (in 'wbvectors') is a matrix
%         of requested pulses wherein each pulse is a column with the first
%         requested pulse in column 1, second requested pulse in column 2,
%         etc.
%                      pulses ->
%         samples |  | 1 2             |
%                 v  | 1 2 ...         |
%                    | 1 2             |
%      PER_PULSE_METADATA: MATLAB structure contain metadata for each
%         individual pulse.
%
%      Examples:
%         Read all data from all pulses, samples, and channels
%         [wbvectors, nbdata] = readerobj.read_cphd(filename,'all','all','all');
%
%         Read every other pulse and samples 1-1000 from channel number 4
%         [wbvectors, nbdata] = readerobj.read_cphd(filename,...
%                                                   first_pulse:2:last_pulse,
%                                                   1:1000,
%                                                   4);
%
%         Read only the narrowband data.  Will read preamble and all per-pulse
%         metadata, but WBVECTORS will be empty since no samples were requested.
%         [wbvectors, nbdata] = readerobj.read_cphd(filename,'all',[]);
%
% [wbvectors, per_pulse_metadata] = readerobj.read_raw(pulse_indices, sample_indices, channels)
%      Reads raw pulse data without any phase stabilization/motion
%      compensation.  Not available for file formats that area already
%      motion compensated.  Input and output arguments are the same as for
%      the READ_CPHD method.
% readerobj.get_meta() % Returns a MATLAB structure with the CPHD
%         "preamble" metadata
% readerobj.close() % Should always be run after all reading is finished
%
% Written by: Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

%% Determine format and open
format_string=guess_ph_format(filename);
if isempty(format_string)
    error('OPEN_PH_READER:UnrecognizedFormat','Unable to determine format of phase history file.');
end
readerobj = eval(['open_' format_string '_reader(''' filename ''', varargin{:});']);

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function [bbs, labels] = acfDetectBadacost( I, detector, fileName)
% Run aggregate channel features object detector on given image(s).
%
% The input 'I' can either be a single image (or filename) or a cell array
% of images (or filenames). In the first case, the return is a set of bbs
% where each row has the format [x y w h score] and score is the confidence
% of detection. If the input is a cell array, the output is a cell array
% where each element is a set of bbs in the form above (in this case a
% parfor loop is used to speed execution). If 'fileName' is specified, the
% bbs are saved to a comma separated text file and the output is set to
% bbs=1. If saving detections for multiple images the output is stored in
% the format [imgId x y w h score] and imgId is a one-indexed image id.
%
% A cell of detectors trained with the same channels can be specified,
% detected bbs from each detector are concatenated. If using multiple
% detectors and opts.pNms.separate=1 then each bb has a sixth element
% bbType=j, where j is the j-th detector, see bbNms.m for details.
%
% USAGE
%  bbs = acfDetectBadacostTrees( I, detector, [fileName] )
%
% INPUTS
%  I          - input image(s) of filename(s) of input image(s)
%  detector   - detector(s) trained via acfTrainBadacostTrees
%  fileName   - [] target filename (if specified return is 1)
%
% OUTPUTS
%  bbs        - [nx5] array of bounding boxes or cell array of bbsc
%  labels     - [nx1] array of subclass labels (for example orientation
%                     class in car detection
%
% EXAMPLE
%
% See also acfTrain, acfModify, bbGt>loadAll, bbNms
%
% Piotr's Computer Vision Matlab Toolbox      Version 3.40
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

% run detector on every image
if(nargin<3), fileName=''; end; multiple=iscell(I);
if(nargin<4), aRatio=[]; end;
if(~isempty(fileName) && exist(fileName,'file')), bbs=1; return; end
if(~multiple), [bbs, labels]=acfDetectImgBadacost(I,detector); else
  n=length(I); bbs=cell(n,1); labels=cell(n,1);
  parfor i=1:n, [bbs{i}, labels{i}]=acfDetectImgBadacost(I{i},detector); disp(i); end
end

% write results to disk if fileName specified
if(isempty(fileName)), return; end
d=fileparts(fileName); if(~isempty(d)&&~exist(d,'dir')), mkdir(d); end
if( multiple ) % add image index to each bb and flatten result
  for i=1:n, bbs{i}=[ones(size(bbs{i},1),1)*i bbs{i}]; end
  are_empty = cellfun(@(x) ((size(x,1) == 0) || size(x,2)==0),bbs);
  bbs(are_empty) = [];
  are_empty = cellfun(@(x) ((size(x,1) == 0) || size(x,2)==0),labels);
  labels(are_empty) = [];
  bbs=cell2mat(bbs);
  labels=cell2mat(labels);
end
dlmwrite(fileName,bbs); bbs=1;

end

function [bbs, labels] = acfDetectImgBadacost( I, detector ) %, aRatio )
% Run trained sliding-window object detector on given image.
Ds=detector; if(~iscell(Ds)), Ds={Ds}; end; nDs=length(Ds);
opts=Ds{1}.opts; pPyramid=opts.pPyramid; pNms=opts.pNms;
imreadf=opts.imreadf; imreadp=opts.imreadp;
shrink=pPyramid.pChns.shrink; pad=pPyramid.pad;
separate=nDs>1 && isfield(pNms,'separate') && pNms.separate;
% read image and compute features (including optionally applying filters)
if(all(ischar(I))), I=feval(imreadf,I,imreadp{:}); end
P=chnsPyramid(I,pPyramid); bbs=cell(P.nScales,nDs);
if(isfield(opts,'filters') && ~isempty(opts.filters)), shrink=shrink*2;
  for i=1:P.nScales, fs=opts.filters; C=repmat(P.data{i},[1 1 size(fs,4)]);
    for j=1:size(C,3), C(:,:,j)=conv2(C(:,:,j),fs(:,:,j),'same'); end
    P.data{i}=imResample(C,.5);
  end
end
% apply sliding window classifiers
for i=1:P.nScales
  for j=1:nDs, opts=Ds{j}.opts;
    modelDsPad=opts.modelDsPad; modelDs=opts.modelDs;

    % -- Use (with changed sign) min positive class cost minus negative class cost as score
    % WARNING: Change to accomodate with theory. We transpose Cprime because acfDetectBadacostTrees1
    %          internally transposes the cost matrix that is passed to it and we want that the
    %          actual matrix used is Cprime, transpose(transpose(Cprime)) => Cprime (2016/11)
    clf_aux = Ds{j}.clf;
    clf_aux.Cprime = clf_aux.Cprime';
    [bb, labels] = acfDetectBadacostTrees1(P.data{i},clf_aux,shrink,...
        modelDsPad(1),modelDsPad(2),opts.stride,opts.cascThr);
    % End WARNING.

    shift=(modelDsPad-modelDs)/2-pad;
    bb(:,1)=(bb(:,1)+shift(2))/P.scaleshw(i,2);
    bb(:,2)=(bb(:,2)+shift(1))/P.scaleshw(i,1);
    bb(:,3)=modelDs(2)/P.scales(i);
    bb(:,4)=modelDs(1)/P.scales(i);
    bb(:,6)=labels(:);  % We make the label of the subclass the 6th argument.
    if(separate), bb(:,7)=j; end; bbs{i,j}=bb;
  end;
end; bbs=cat(1,bbs{:});

if(isfield(Ds{1}.clf, 'aRatio'))
  % JMBUENA: Change bounding boxes to class specific bounding box. For
  % example in the KITTI benchmark we have cars. At each orientation the
  % bounding box of the car has a specific aspect ratio (a size view of the
  % car is rectangular and a frontal car is squared).
  use_fixed_width = 0;
  if isfield(Ds{1}.clf, 'aRatioFixedWidth')
    use_fixed_width = Ds{1}.clf.aRatioFixedWidth;
  end
  bbs = correctToClassSpecificBbs(bbs, Ds{1}.clf.aRatio, use_fixed_width, 1);
end

if(~isempty(pNms)), bbs=bbNms(bbs,pNms); end
end


function urdf = urdfparse(filename)

    root = xmlread(filename);

    robot = root.getElementsByTagName('robot').item(0);

    %% process all the materials elements
    % - these are optional but we do them first because link elements
    %   reference them
    materialNodes = getChildrenByTagName(robot, 'material');
    materials = [];
    materialNames = [];
    if ~isempty(materialNodes)

        for i = 1:length(materialNodes)
            node = materialNodes(i);
            materials(i).name = string(node.getAttribute('name'));
            materials(i).rgba = str2num(node.getElementsByTagName('color').item(0).getAttribute('rgba'));
            materials(i).node = node;
        end

        urdf.materials = materials;
        materialNames = [materials.name];
    end

    %% process all the link elements

    % create a vector of joint structures, fields are:
    % name       name of the joint
    % type       type of joint (fixed, continuous, prismatic)
    % T          SE3 object for pose of joint wrt link frame
    % xyz        position of joint wrt link frame
    % rpy        RPY angles of joint frame wrt link frame
    % node       reference to XML DOM node for this <joint>
    % child      link index for downstream link
    % parent     link index for downstream link

    linkNodes = getChildrenByTagName(robot, 'link');

    for i = 1:length(linkNodes)
        node = linkNodes(i);
        links(i).name = string(node.getAttribute('name'));
        try
            links(i).geometry = node.getElementsByTagName('geometry').item(0);
            links(i).mesh = links(i).geometry.getElementsByTagName('mesh').item(0).getAttribute('filename');
        end
        try
            links(i).material = find(strcmp((node.getElementsByTagName('material').item(0).getAttribute('name')), materialNames));
        end
        try
            inertial = node.getElementsByTagName('inertial').item(0);
            try
                links(i).m = str2num(inertial.getElementsByTagName('mass').item(0).getAttribute('value'));
            end
            try
                I = inertial.getElementsByTagName('inertia').item(0);
                links(i).I.xx = str2num(I.getAttribute('ixx'));
                links(i).I.xy = str2num(I.getAttribute('ixy'));
                links(i).I.xz = str2num(I.getAttribute('ixz'));
                links(i).I.yy = str2num(I.getAttribute('iyy'));
                links(i).I.yz = str2num(I.getAttribute('iyz'));
                links(i).I.zz = str2num(I.getAttribute('izz'));
            end
            try
                t = str2num( node.getElementsByTagName('origin').item(0).getAttribute('xyz'));
                rpy = str2num( node.getElementsByTagName('origin').item(0).getAttribute('rpy'));
                links(i).com = SE3(t) * SE3.rpy(rpy);
            end
        end
        t = [0 0 0]; rpy = [0 0 0]; % in case not provided
        try
            t = str2num( node.getElementsByTagName('origin').item(0).getAttribute('xyz'));
            rpy = str2num( node.getElementsByTagName('origin').item(0).getAttribute('rpy'));
        end
        if isempty(t)
            t = [0 0 0]; % in case the tag has no attribute
        end
        if isempty(rpy)
            rpy = [0 0 0];  % in case the tag has no attribute
        end
        links(i).T = SE3(t) * SE3.rpy(rpy);
        links(i).node = node;
        links(i).children = [];
        links(i).parent = [];
    end

    linkNames = [links.name];  % a list of link names

    %% process all the joint elements

    % create a vector of link structures, fields are are:
    % name       name of the link
    % geometry   reference to XML DOM node
    % T          SE3 object for origin of visual wrt link frame
    % node       reference to XML DOM node for this <link>
    % children   list of joint indexes attaching this link to its children
    % parent     index of joint attaching this link to its parent

    jointNodes = getChildrenByTagName(robot, 'joint');
    joints = [];
    for i = 1:length(jointNodes)
        node = jointNodes(i);
        joints(i).name = string(node.getAttribute('name'));
        joints(i).type = string(node.getAttribute('type'));
        try
            joints(i).axis = str2num(node.getElementsByTagName('axis').item(0).getAttribute('xyz'));
        end
        p = find(strcmp(node.getElementsByTagName('parent').item(0).getAttribute('link'), linkNames));
        c = find(strcmp(node.getElementsByTagName('child').item(0).getAttribute('link'), linkNames));
        joints(i).parent = p;
        joints(i).child = c;
        links(p).children = [links(p).children i];
        links(c).parent = i;

        t = [0 0 0]; rpy = [0 0 0];  % in case not provided
        try
            t = str2num( node.getElementsByTagName('origin').item(0).getAttribute('xyz'));
            rpy = str2num( node.getElementsByTagName('origin').item(0).getAttribute('rpy'));
        end
        if isempty(t)
            t = [0 0 0]; % in case the tag has no attribute
        end
        if isempty(rpy)
            rpy = [0 0 0]; % in case the tag has no attribute
        end
        joints(i).T = SE3(t) * SE3.rpy(rpy);
        joints(i).xyz = t;
        joints(i).rpy = rpy;
        joints(i).node = node;
    end
    urdf.joints = joints;
    urdf.links = links;

    urdf.endlinks = find( cellfun('isempty', {links.children}) );
    urdf.ets = [];

    %% create a transform list

    % the mechanism might have multiple endpoints (if branched), for each
    % endpoint compute an ETS chain and return a string array

    for endlink = urdf.endlinks
        transforms = "";
        joint = joints(links(endlink).parent);
        while true
            transforms = xform2s(joint) + axis2s(joint) + transforms;
            joint = joints(links(joint.parent).parent);
            if isempty(joint)
                break;
            end
        end
        transforms = strtrim(transforms);
        urdf.ets = [urdf.ets transforms];
    end

    %% display a summary
    fprintf('\nLinks::\n');
    for i=1:length(links)
        name = links(i).name;
        if ismember(i, urdf.endlinks)
            name = "*" + name;
        end
        geomlist = "";
        if ~isempty(links(i).geometry)
            g = links(i).geometry.getChildNodes;
            for k=0:g.getLength-1
                c = g.item(k);
                if c.getNodeType == 1  % skip text nodes
                    if geomlist == ""
                        geomlist = "shape=";
                    else
                        geomlist = geomlist + ", ";
                    end
                    geomlist = geomlist + string(c.getNodeName);
                end
            end
        end
        fprintf('%2d: %10s; parentjoint=%s, childjoints=%s; %s\n', i, name, ...
            formatj(links(i).parent), formatj(links(i).children), geomlist);
    end
    if ~isempty(joints)
        fprintf('\nJoints::\n');
        for i=1:length(joints)
            fprintf('%2d: %10s; type=%s, parentlink=%d, childlink=%d\n', i, joints(i).name, joints(i).type, joints(i).parent, joints(i).child);
        end
    end
    if ~isempty(materials)
        fprintf('\nMaterials::\n');
        for i=1:length(materials)
            fprintf('%10s: %f %f %f %f\n', materials(i).name, materials(i).rgba);
        end
    end
    if strlength(transforms) > 0
        fprintf('\nForward kinematics::\n  %s\n', transforms);
    end

end

function s = formatj(j)
    if isempty(j)
        s = '-';
    else
        s = strip(num2str(j, '%d,'), 'right', ',');
    end
end

function t = xform2s(joint)

    t = "";
    if joint.xyz(1) ~= 0
        t = t + sprintf('Tx(%.12g) ', joint.xyz(1));
    end
    if joint.xyz(2) ~= 0
        t = t + sprintf('Ty(%.12g) ', joint.xyz(2));
    end
    if joint.xyz(3) ~= 0
        t = t + sprintf('Tz(%.12g) ', joint.xyz(3));
    end
    if joint.rpy(3) ~= 0
        t = t + sprintf('Rz(%.12g) ', round(joint.rpy(3)*180/pi, 3));
    end
    if joint.rpy(2) ~= 0
        t = t + sprintf('Ry(%.12g) ', round(joint.rpy(2)*180/pi,3));
    end
    if joint.rpy(1) ~= 0
        t = t + sprintf('Rx(%.12g) ', round(joint.rpy(1)*180/pi, 3));
    end
end

function t = axis2s(joint)

    t = "";
    switch joint.type
        case {'revolute', 'continuous'}
            assert(length(find(joint.axis)) == 1, 'Joint axis must be about x, y or z only');
            if joint.axis(1) ~= 0
                t = t + sprintf('Rx(%s) ', joint.name);
            end
            if joint.axis(2) ~= 0
                t = t + sprintf('Ry(%s) ', joint.name);
            end
            if joint.axis(3) ~= 0
                t = t + sprintf('Rz(%s) ', joint.name);
            end
        case 'prismatic'
            assert(length(find(joint.axis)) == 1, 'Joint axis must be about x, y or z only');
            if joint.axis(1) ~= 0
                t = t + sprintf('Tx(%s) ', joint.name);
            end
            if joint.axis(2) ~= 0
                t = t + sprintf('Ty(%s) ', joint.name);
            end
            if joint.axis(3) ~= 0
                t = t + sprintf('Tz(%s) ', joint.name);
            end
        case 'fixed'
    end

end

function children = getChildrenByTagName(parent, name)
    % return array of immediate child nodes with node name equal to name
    % REF https://stackoverflow.com/questions/18776408/get-all-the-children-for-a-given-xml-in-java
    children = [];
    childnodes = parent.getChildNodes;
    for i=1:childnodes.getLength
        node = childnodes.item(i-1);
        if node.getNodeType == node.ELEMENT_NODE && strcmp(node.getNodeName, name)
            children = [children node];
        end
    end
end


% Takes a hsvargplvm model and adds a non-(standard normal) prior on the
% parent (see: addDynamics functions in the other vargplvm-related
% packages).

% See also: svargplvmAddDynamics.m

function model = hsvargplvmAddParentPrior(model, globalOpt, optionsDyn)

modelParent = model.layer{model.H};
modelParent.comp{1}.vardist = modelParent.vardist;
modelParent.comp{1}.X = modelParent.vardist.means;
% TODO: if center means....
if model.H > 1
    modelParent.comp{1}.y = model.layer{end-1}.vardist.means;
elseif length(model.layer{1}.comp) == 1
    modelParent.comp{1}.y = model.layer{1}.comp{1}.y;
else
    error('Not implemented!')
end

modelParent.comp{1} = svargplvmAddDynamics(modelParent.comp{1}, globalOpt, optionsDyn);

modelParent.vardist = modelParent.comp{1}.vardist;
modelParent.dynamics = modelParent.comp{1}.dynamics;
modelParent.comp{1} = rmfield(modelParent.comp{1}, 'vardist');
modelParent.comp{1} = rmfield(modelParent.comp{1}, 'dynamics');
modelParent.comp{1} = rmfield(modelParent.comp{1}, 'X');
modelParent.comp{1} = rmfield(modelParent.comp{1}, 'y');

model.layer{model.H} = modelParent;

%params = hsvargplvmExtractParam(model);
%model = hsvargplvmExpandParam(model, params);


function [tracker, qscore, f] = track_frame(tracker, frame_id, frame_image, bboxes_det, index_det, seq_name, opt)

    global client_tcp;

    % tracked, decide to tracked or occluded
    if tracker.state == opt.STATE_TRACKED || tracker.state == opt.STATE_ACTIVATED
        tracker = ECO_tracking(frame_id, frame_image, bboxes_det, tracker, opt);
        is_tracking_score_good = tracker.eco.is_confident;
        is_overlap_ratio_good = mean(tracker.eco.bb_overlaps);
        tracker.bb = tracker.eco.bb';
        qscore = 0;
        if is_tracking_score_good == 1
            if seq_name == 'MOT16-03'
                overlap_threshold = 0.1;
            else
                overlap_threshold = 0.8;
            end
            if tracker.state == opt.STATE_TRACKED && is_overlap_ratio_good > overlap_threshold
                label = 1;
            elseif tracker.state == opt.STATE_ACTIVATED %&& is_overlap_ratio_good > 0.8
                label = 1;
            else
                label = -1;
            end
        else
            label = -1;
        end

        % make a decision
        if label > 0
            if tracker.num_tracked > (0.25 * tracker.fps)
                tracker.state = opt.STATE_TRACKED;
            else
                tracker.state = opt.STATE_ACTIVATED;
            end
        else
            if tracker.state == opt.STATE_ACTIVATED
                tracker.state = opt.STATE_STOP;
                fprintf('Target %d end.\n', tracker.target_id);
            else
                tracker.state = opt.STATE_LOST;
            end
        end

        % build the bboxes structure new
        if tracker.state == opt.STATE_TRACKED || tracker.state == opt.STATE_ACTIVATED
            bbox.x = tracker.bb(1);
            bbox.y = tracker.bb(2);
            bbox.w = tracker.bb(3) - tracker.bb(1);
            bbox.h = tracker.bb(4) - tracker.bb(2);
            bbox.r = 1;
            bbox.eco_x = tracker.eco.bb(1);
            bbox.eco_y = tracker.eco.bb(2);
            bbox.eco_w = tracker.eco.bb(3) - tracker.eco.bb(1);
            bbox.eco_h = tracker.eco.bb(4) - tracker.eco.bb(2);

            if ~isempty(bboxes_det.fr)
                o = calc_overlap(bbox, 1, bboxes_det, 1:numel(bboxes_det.fr));
                tracker.eco.bb_overlaps(1:end-1) = tracker.eco.bb_overlaps(2:end);
                if max(o) > 0.5
                    tracker.eco.bb_overlaps(end) = 1;
                else
                    tracker.eco.bb_overlaps(end) = 0;
                end
            else
                tracker.eco.bb_overlaps(1:end-1) = tracker.eco.bb_overlaps(2:end);
                tracker.eco.bb_overlaps(end) = 0;
            end
        else
            bbox = sub_bboxes(tracker.bboxes, numel(tracker.bboxes.fr));
        end
        if tracker.state == opt.STATE_ACTIVATED && tracker.eco.bb_overlaps(end) == 0
            tracker.state = opt.STATE_STOP;
        end
        bbox.fr = frame_id;
        bbox.id = tracker.target_id;
        bbox.state = tracker.state;
        tracker.bboxes = concatenate_bboxes(tracker.bboxes, bbox);

    % occluded, decide to tracked or occluded
    elseif tracker.state == opt.STATE_LOST
        % association
        if isempty(index_det)
            qscore = 0;
            label = -1;
            f = [];
        else
            bboxes = sub_bboxes(bboxes_det, index_det);
            ctrack = motion_predict(frame_id, tracker, opt);
            bboxes_predict.w = tracker.bboxes.w(end);
            bboxes_predict.h = tracker.bboxes.h(end);
            bboxes_predict.x = ctrack(1) - (bboxes_predict.w - 1) / 2;
            bboxes_predict.y = ctrack(2) - (bboxes_predict.h - 1) / 2;
            o_predict = calc_overlap(bboxes_predict, 1, bboxes, 1:numel(bboxes.fr));
            [motion_score, motion_ind] = max(o_predict);
            bboxes_tracker = tracker.bboxes;
            index_state = find(bboxes_tracker.state == opt.STATE_TRACKED | bboxes_tracker.state == opt.STATE_ACTIVATED);
            bboxes_tracker = sub_bboxes(bboxes_tracker, index_state);
            if ~isempty(bboxes_tracker.fr)
                fr_tracker = bboxes_tracker.fr(end);
            else % for target which is suppressed since the first frame it appears
                fr_tracker = 1;
            end
            traj_dir = ['img_traj/' seq_name '/' num2str(tracker.target_id) '/'];
            frame_id_double = double(frame_id);
            save('mot_py.mat', 'traj_dir', 'bboxes', 'frame_id_double', 'seq_name');

            fwrite(client_tcp, 'client ok');
            fread(client_tcp, 9); % size is 9 for 'server ok' message
            load('similarity.mat');

            [ass_score, ind] = max(similarity);
            index_tracked = find(tracker.bboxes.state == opt.STATE_TRACKED | tracker.bboxes.state == opt.STATE_ACTIVATED);
            bboxes_tracked = sub_bboxes(tracker.bboxes, index_tracked);
            fr_tracked = bboxes_tracked.fr(end);
            if frame_id - fr_tracker == 1
                prev_c = [bboxes_tracked.x(end) + bboxes_tracked.w(end) / 2, bboxes_tracked.y(end) + bboxes_tracked.h(end) / 2];
                prev_w = bboxes_tracked.w(end);
                prev_h = bboxes_tracked.h(end);
                bboxes_det_one = sub_bboxes(bboxes_det, index_det(ind));
                cur_c = [bboxes_det_one.x + bboxes_det_one.w/2, bboxes_det_one.y + bboxes_det_one.h/2];
                dis = norm(cur_c - prev_c) / prev_w;
                ratio = bboxes_det_one.h / prev_h;
                ratio = min(ratio, 1/ratio);
            end

            if ass_score > opt.association_score_thre
                label = 1;
                bboxes_det_one = sub_bboxes(bboxes_det, index_det(ind));
                fprintf('Target %d associated by appearance.\n', tracker.target_id);

            elseif frame_id - fr_tracker < 5 && ass_score > 0.4 && motion_score > 0.5
                label = 1;
                bboxes_det_one = sub_bboxes(bboxes, motion_ind);
                fprintf('Target %d associated by motion.\n', tracker.target_id);
            else
                label = -1;
            end

            if label == 1
                tracker.eco.rect_position = double(round([bboxes_det_one.x, bboxes_det_one.y, bboxes_det_one.w, bboxes_det_one.h]));
                tracker.eco.pos = single([bboxes_det_one.y + (bboxes_det_one.h - 1)/2, bboxes_det_one.x + (bboxes_det_one.w - 1)/2]);
                tracker.eco.target_sz = double([bboxes_det_one.h, bboxes_det_one.w]);
                tracker.eco.base_target_sz = double(tracker.eco.target_sz / tracker.eco.currentScaleFactor);
                tracker.eco.base_target_sz(2) = tracker.eco.base_target_sz(2)* 0.5;
                tracker = ECO_tracking(frame_id, frame_image, bboxes_det_one, tracker, opt);
            end
        end

        if label > 0
            tracker.bb = tracker.eco.bb';

            % association
            if tracker.num_tracked > 0.25 * tracker.fps
                tracker.state = opt.STATE_TRACKED;
            else
                tracker.state = opt.STATE_ACTIVATED;
            end

            % build the bboxes structure
            bbox = [];
            bbox.fr = frame_id;
            bbox.id = tracker.target_id;
            bbox.x = tracker.bb(1);
            bbox.y = tracker.bb(2);
            bbox.w = tracker.bb(3) - tracker.bb(1);
            bbox.h = tracker.bb(4) - tracker.bb(2);
            bbox.r = 1;
            bbox.state = tracker.state;
            bbox.eco_x = tracker.eco.bb(1);
            bbox.eco_y = tracker.eco.bb(2);
            bbox.eco_w = tracker.eco.bb(3) - tracker.eco.bb(1);
            bbox.eco_h = tracker.eco.bb(4) - tracker.eco.bb(2);

            if tracker.bboxes.fr(end) == frame_id
                bboxes = tracker.bboxes;
                index = 1:numel(bboxes.fr)-1;
                tracker.bboxes = sub_bboxes(bboxes, index);
            end

            tracker.bboxes = interpolate_traj(tracker.bboxes, bbox, tracker.fps, opt);

            if isempty(bboxes_det.fr) == 0
                o = calc_overlap(bbox, 1, bboxes_det, 1:numel(bboxes_det.fr));
                tracker.eco.bb_overlaps(1:end-1) = tracker.eco.bb_overlaps(2:end);
                if max(o) > 0.5
                    tracker.eco.bb_overlaps(end) = 1;
                else
                    tracker.eco.bb_overlaps(end) = 0;
                end
            else
                tracker.eco.bb_overlaps(1:end-1) = tracker.eco.bb_overlaps(2:end);
                tracker.eco.bb_overlaps(end) = 0;
            end
        else
            % no association
            tracker.state = opt.STATE_LOST;
            bbox = sub_bboxes(tracker.bboxes, numel(tracker.bboxes.fr));
            bbox.fr = frame_id;
            bbox.id = tracker.target_id;
            bbox.state = tracker.state;

            if tracker.bboxes.fr(end) == frame_id
                bboxes = tracker.bboxes;
                index = 1:numel(bboxes.fr)-1;
                tracker.bboxes = sub_bboxes(bboxes, index);
            end
            tracker.bboxes = concatenate_bboxes(tracker.bboxes, bbox);
        end
    end


function out = double(fa)
% Convert to double precision
% FORMAT double(fa)
% fa - a file_array
% _______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

%
% $Id$

out = double(numeric(fa));


classdef TestBm3dDenoising
    %TestBm3dDenoising

    methods (Static)
        function test_1
            img = cv.imread(fullfile(mexopencv.root(),'test','cat.jpg'), ...
                'Grayscale',true, 'ReduceScale',2);
            out = cv.bm3dDenoising(img, 'H',10);
            validateattributes(out, {class(img)}, {'size',size(img)});
        end

        function test_error_argnum
            try
                cv.bm3dDenoising();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


function polygon = setLMpolygon(x,y)

polygon.x = x(:);
polygon.y = y(:);

%for i = 1:length(x)
%    polygon.pt(i).x = num2str(x(i));
%    polygon.pt(i).y = num2str(y(i));
%end


function varargout = equalizer(varargin)
% EQUALIZER MATLAB code for equalizer.fig
%      EQUALIZER, by itself, creates a new EQUALIZER or raises the existing
%      singleton*.
%
%      H = EQUALIZER returns the handle to a new EQUALIZER or the handle to
%      the existing singleton*.
%
%      EQUALIZER('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in EQUALIZER.M with the given input arguments.
%
%      EQUALIZER('Property','Value',...) creates a new EQUALIZER or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before equalizer_OpeningFcn gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to equalizer_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help equalizer

% Last Modified by GUIDE v2.5 13-Jan-2012 17:19:05

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @equalizer_OpeningFcn, ...
                   'gui_OutputFcn',  @equalizer_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before equalizer is made visible.
function equalizer_OpeningFcn(hObject, eventdata, handles, varargin)
global s Fs
global g1 g2 n hs
global po
global pla pau
global hdls
global vol
global Q f filts_type
global hp hpp

% g1 g2 limits og gains
% po buffer size in seconds

% s - signal
% Fs - sampling frequency
% g1 g2 limits og gains
% po buffer size in seconds

% Choose default command line output for equalizer
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

hdls=handles; % save all handles to global variable

% UIWAIT makes equalizer wait for user response (see UIRESUME)
% uiwait(handles.figure1);

g1=-10;
g2=10;
n=10; % mber of sliders
f=[60  170 310  600 1e3 3e3   6e3  12e3  14e3  16e3]; % frequencyes
%f=0.7*exp(1:10);
%f=24*exp(1:6);
% f=[43 118  321 873 2374 6454 17540];
% f=[60  310  600 1e3   6e3  12e3   16e3]; % frequencyes
Q=1*ones(1,n); % Q-factors
filts_type=[1 2*ones(1,n-2) 3]; % filt types: 1-low shelving, 2-peak, 3-high shelving

po=0.2; % buffers size, seconds
pla=false; % true if play
pau=false; % true if pause

vol=0.7; % volume
ps=get(handles.szg,'position');
pu=get(handles.szg,'units');

sz=ps(4);

set(handles.vol,'value',vol);

hp=semilogx(NaN,NaN,'r-','parent',handles.axes1);
set(handles.axes1,'NextPlot','add');
hpp=semilogx(NaN,NaN,'ok','parent',handles.axes1);
xlabel(handles.axes1,'frequrency, Hz');
ylabel(handles.axes1,'gain, dB');

pg1=get(handles.g1,'position');
pg2=get(handles.g2,'position');
szy=pg2(2)-pg1(2);

x=ps(1)+sz;
y=ps(2)-1*sz-szy;

pa=get(handles.axes1,'position');
sxa=pa(3);

hs=zeros(1,n);

for nc=1:n
    hs(nc)=uicontrol('Style', 'slider',...
            'Min',g1,'Max',g2,'Value',0,...
            'units',pu,...
            'Position', [x y 1.5*sz szy],...
            'Callback', 'recalculate_filter;');

    x=x+sxa/n;
end

Fs=44100;
recalculate_filter;

% --- Outputs from this function are returned to the command line.
function varargout = equalizer_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

function file_Callback(hObject, eventdata, handles)
fln=get(handles.file,'string');
set(handles.reading,'visible','on');
drawnow;
read_audio(fln);
set(handles.reading,'visible','off');
drawnow;

% --- Executes during object creation, after setting all properties.
function file_CreateFcn(hObject, eventdata, handles)
% hObject    handle to file (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

% --- Executes on button press in browse.
function browse_Callback(hObject, eventdata, handles)
[FileName,PathName,FilterIndex] = uigetfile({'*.mp3;*.wav;*.wma;*.avi;*.mpg','media files (*.mp3,*.wav,*.wma,*.avi,*.mpg)';'*.*',  'All Files (*.*)'},'File Selector') ;
isok=true;
if length(FileName)==1
     if FileName==0
         isok=false;
     end
end
if isok
    fln=[PathName FileName];
    set(handles.file,'string',fln);
    set(handles.reading,'visible','on');
    drawnow;
    read_audio(fln);
    set(handles.reading,'visible','off');
    drawnow;

end

% --- Executes on slider movement.
function slider1_Callback(hObject, eventdata, handles)
% hObject    handle to slider1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
% disp('slider releized');

% --- Executes during object creation, after setting all properties.
function slider1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to slider1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end

% --- Executes on slider movement.
function vol_Callback(hObject, eventdata, handles)
% hObject    handle to vol (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

global vol
vol=get(hObject,'Value');

% --- Executes during object creation, after setting all properties.
function vol_CreateFcn(hObject, eventdata, handles)
% hObject    handle to vol (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end

% --- Executes on button press in szg.
function szg_Callback(hObject, eventdata, handles)
global n hs
for nc=1:n
    set(hs(nc),'value',0);
end
drawnow;
recalculate_filter;

% --- Executes on button press in play.
function play_Callback(hObject, eventdata, handles)
open_device;

% --- Executes on button press in pause.
function pause_Callback(hObject, eventdata, handles)
global pau
pau=~pau;
if pau
    set(handles.pause,'string','Unpause');
    drawnow;
else
    set(handles.pause,'string','Pause');
    drawnow;
end

% --- Executes on button press in stop.
function stop_Callback(hObject, eventdata, handles)
global pla
pla=false;

% --- Executes on button press in sset.
function sset_Callback(hObject, eventdata, handles)
global po pso pco s
L=size(s,1);
v=get(handles.slider1,'value');
pco=round(v*L/pso);
if pco<1
    pco=1;
end


function D = DateStr2Num(S, F)
% DATESTR2NUM - Fast conversion of DATESTR to DATENUM
% The builtin DATENUM command is very powerful, but if you know the input
% format exactly, a specific MEX can be much faster: e.g. for single strings
% DateStr2Num is 50 to 100 times faster than DATENUM, for a {1 x 10000} cell
% string the speed up factor is 300 to 700 (Matlab 2009a/2011b/64, MSVC 2008).
%
% D = DateStr2Num(S, F)
% INPUT:
%   S: String or cell string in DATESTR(F) format.
%      In opposite to DATENUM the validity of the input string is not checked
%      (e.g. 1 <= month <= 12).
%   F: Integer number defining the input format. Accepted:
%          0:  'dd-mmm-yyyy HH:MM:SS'      01-Mar-2000 15:45:17
%          1:  'dd-mmm-yyyy'               01-Mar-2000
%         29:  'yyyy-mm-dd'                2000-03-01
%         30:  'yyyymmddTHHMMSS'           20000301T154517
%         31:  'yyyy-mm-dd HH:MM:SS'       2000-03-01 15:45:17
%      Including the milliseconds (not a valid DATEFORM number in DATESTR!):
%        1000: 'dd-mmm-yyyy HH:MM:SS.FFF'  01-Mar-2000 15:45:17.123
%        1030: 'yyyymmddTHHMMSS.FFF'       20000301T154517.123
%
%      Optional, default: 0.
%
% OUTPUT:
%   D: Serial date number. If S is a cell, D has is same size.
%
% EXAMPLES:
%   C = {'2010-06-29 21:59:13', '2010-06-29 21:59:13'};
%   D = DateStr2Num(C, 31)
%   >> [734318.916122685, 734318.916122685]
%   Equivalent Matlab command (but a column vector is replied ever):
%   D = datenum(C, 'yyyy-mm-dd HH:MM:SS')
%
% NOTES: The parsing of the strings works for clean ASCII characters only:
%   '0' must have the key code 48!
%   Month names must be English with the 2nd and 3rd charatcer in lower case.
%
% COMPILATION:
% The C-function must be compiled before using. The M-file produces the same
% results, but is slower.
%   Windows: mex -O DateStr2Num.c
%   Linux:   mex -O CFLAGS="\$CFLAGS -std=c99" DateStr2Num.c
%   Download precompiled Mex: http://www.n-simon.de/mex
%
% Tested: Matlab 6.5, 7.7, 7.8, 7.13, WinXP/32, Win7/64
% Compiler: LCC 2.4/3.8, BCC 5.5, Open Watcom 1.8, MSVC 2008
%         Compatibility to MacOS, Linux, 64 bit is assumed, but not tested.
% Author: Jan Simon, Heidelberg, (C) 2010-2013 matlab.THISYEAR(a)nMINUSsimon.de
%
% See also DATESTR, DATENUM, DATEVEC.
% FEX: DateConvert 25594 (Jan Simon)

% $JRev: R-h V:008 Sum:gZffjdt4JfVU Date:28-Jul-2013 15:03:20 $
% $License: BSD (use/copy/change/redistribute on own risk, mention the author) $
% $UnitTest: uTest_DateStr2Num $
% $File: Tools\GLString\DateStr2Num.m $
% History:
% 001: 29-Jun-2010 22:07, First version.
% 002: 30-Jun-2010 16:08, Accept formats 0, 1, 29, 30, 31.
% 008: 28-Jul-2013 12:48, 300 -> 1030, format 1000 added.

% ==============================================================================
% The MEX is not found?! Perhaps the parent folder of this function is the
% current folder...
% peristent WarnOnce     % Uncomment this to warn once only
% if isempty(WarnOnce)
warning('JSimon:DateStr2Num:NoMex', ...
   ['The slow M-version of DateStr2Num is used\n', ...
   'Read the help text -> COMPILATION']);
%   WarnOnce = true;
% end

% Compared to DATENUM, this M-version is 20 times faster than DATENUM(S, 31) for
% a string and 1.1 to 1.4 times slower for a {1 x 10000} cell string.
if ischar(S)
   S = {S};
elseif ~iscellstr(S)
   error('JSimon:DateStr2Num:BadInputType', ...
      'Input must be a string or cell string.');
end

if nargin == 1
   F = 0;
end

nS = numel(S);
D  = zeros(size(S));
switch F
   case 0  % 'dd-mmm-yyyy HH:MM:SS'
      for iS = 1:nS
         aS    = S{iS};
         x     = aS - '0';
         month = (strfind('janfebmaraprmayjunjulaugsepoctnovdec', ...
            lower(aS(4:6))) + 2) / 3;
         year  = x(8) * 1000 + x(9) * 100 + x(10) * 10 + x(11);

         D(iS) = datenummx(year, month, x(1) * 10 + x(2), ...
            x(13) * 10 + x(14), x(16) * 10 + x(17), ...
            x(19) * 10 + x(20));
      end

   case 1  % 'dd-mmm-yyyy'
      for iS = 1:nS
         aS    = S{iS};
         x     = aS - '0';
         month = (strfind('janfebmaraprmayjunjulaugsepoctnovdec', ...
            lower(aS(4:6))) + 2) / 3;
         year  = x(8) * 1000 + x(9) * 100 + x(10) * 10 + x(11);

         D(iS) = datenummx(year, month, x(1) * 10 + x(2));
      end

   case 29  % 'yyyy-mm-dd'
      for iS = 1:nS
         x     = S{iS} - '0';
         D(iS) = datenummx( ...
            x(1)*1000 + x(2)*100 + x(3)*10 + x(4), ... % Year
            x(6)*10 + x(7), ...  % Month
            x(9)*10 + x(10));     % Day
      end

   case 30  % 'yyyymmddTHHMMSS'
      for iS = 1:nS
         x     = S{iS} - '0';
         D(iS) = datenummx( ...
            x(1)*1000 + x(2)*100 + x(3)*10 + x(4), ... % Year
            x(5)*10 + x(6), ...  % Month
            x(7)*10 + x(8), ...  % Day
            x(10)*10 + x(11), x(12)*10 + x(13), x(14)*10 + x(15));
      end

   case 31  % 'yyyy-mm-dd HH:MM:SS'
      for iS = 1:nS
         x     = S{iS} - '0';
         D(iS) = datenummx( ...
            x(1)*1000 + x(2)*100 + x(3)*10 + x(4), ... % Year
            x(6)*10 + x(7), ...  % Month
            x(9)*10 + x(10), ... % Day
            x(12)*10 + x(13), x(15)*10 + x(16), x(18)*10 + x(19));
      end

   case 1000  % 'dd-mmm-yyyy HH:MM:SS.FFF'
      for iS = 1:nS
         aS    = S{iS};
         x     = aS - '0';
         month = (strfind('janfebmaraprmayjunjulaugsepoctnovdec', ...
            lower(aS(4:6))) + 2) / 3;
         year  = x(8) * 1000 + x(9) * 100 + x(10) * 10 + x(11);

         D(iS) = datenummx(year, month, x(1) * 10 + x(2), ...
            x(13) * 10 + x(14), x(16) * 10 + x(17), ...
            x(19) * 10 + x(20) + ...
            x(22) / 10 + x(23) / 100 + x(24) / 1000);
      end

   case 1030  % yyyymmddTHHMMSS.FFF
      for iS = 1:nS
         x     = S{iS} - '0';
         D(iS) = datenummx( ...
            x(1)*1000 + x(2)*100 + x(3)*10 + x(4), ... % Year
            x(5)*10 + x(6), ...  % Month
            x(7)*10 + x(8), ...  % Day
            x(10)*10 + x(11), x(12)*10 + x(13), x(14)*10 + x(15) + ...
            x(17) / 10 + x(18) / 100 + x(19) / 1000);
      end

   otherwise
      error('JSimon:DateStr2Num:BadFormat', 'Bad format');
end

% return;


function ok = all_set_item(item)

% function ok = all_set_item(item)
% Perform within-item all_set check. For repeats, this is true, if item.val
% has between item.num(1) and item.num(2) elements.
%
% This code is part of a batch job configuration system for MATLAB. See
%      help matlabbatch
% for a general overview.
%_______________________________________________________________________
% Copyright (C) 2007 Freiburg Brain Imaging

% Volkmar Glauche
% $Id: all_set_item.m 1716 2008-05-23 08:18:45Z volkmar $

rev = '$Rev: 1716 $'; %#ok
ok = (numel(item.cfg_item.val) >= item.num(1)) &&...
    (numel(item.cfg_item.val) <= item.num(2));


%ISFIXED Test on fixed mapping
%
%    I = ISFIXED(W)
%    ISFIXED(W)
%
% True if the mapping type of W is 'fixed' (see HELP MAPPINGS). If called
% without an output argument ISFIXED generates an error if the mapping type
% of W is not 'fixed'.


function [wordCounts] = updateCounts(wordCounts, data)
  wordCounts.total = wordCounts.total + data.numWords;
  wordCounts.word = wordCounts.word + data.numWords;
end