function deformation_field = PTKSolveMatchedImagesForFluidRegistration(image_to_transform, reference_image, reporting)
    % PTKSolveMatchedImagesForFluidRegistration.
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    MimImageUtilities.MatchSizes(reference_image, image_to_transform);

    if ~isequal(image_to_transform.VoxelSize, reference_image.VoxelSize)
        reporting.Error('SolveMatchedImagesForFluidRegistration:UnequalVoxelSize', 'SolveMatchedImagesForFluidRegistration requires images to have the same voxel size');
    end

    % Convert to distance transforms
    dt_float = PTKRunForEachComponentAndCombine(@MimImageUtilities.GetNormalisedDT, image_to_transform, image_to_transform, reporting);
    dt_ref = PTKRunForEachComponentAndCombine(@MimImageUtilities.GetNormalisedDT, reference_image, reference_image, reporting);

    % Solve to find the deformation field between these images
    [deformation_field, ~] = Solve(dt_float, dt_ref, reporting);
end

function [deformation_field, deformed_image] = Solve(image_1, image_2, reporting)
    deformation_field = image_2.BlankCopy;
    deformed_image = image_2.BlankCopy;
    voxel_size = image_2.VoxelSize;
    if ~isequal(voxel_size, image_1.VoxelSize)
        reporting.Error('PTKGetFluidDeformationField:DifferentVoxelSizes', 'Images must have the same voxel size');
    end
    if ~isequal(image_1.ImageSize, image_2.ImageSize)
        reporting.Error('PTKGetFluidDeformationField:DifferentVoxelSizes', 'Images must have the same image size');
    end

    if PTKSoftwareInfo.ShowRegistrationConvergence
        display_value = 'iter';
    else
        display_value = 'off';
    end
    iterations = 200;
    % X corresponds to image columns and Y image rows
    options = npRegSet(...
        'Display', display_value, ...
        'Regularizer', 'curvature', ... % elastic | {fluid} | diffusion | curvature
        'BoundaryCond', 'Neumann', ...
        'VoxSizeX', voxel_size(2),'VoxSizeY', voxel_size(1), 'VoxSizeZ', voxel_size(3), ...
        'maxiter', iterations, ...
        'RegularizerFactor', 1,...
        'BodyForceTol', PTKSoftwareInfo.RegistrationBodyForceTol, ... % Termination tolerance on the body force [1e-2]
        'BodyForceDiffTol', PTKSoftwareInfo.RegistrationBodyForceDiffTol, ... % Termination tolerance on the difference between successive body force estimates [1e-2]..
        'SimMeasPercentDiffTol', 0.001, ... % Termination tolerance on the percentage difference between successive similarity measure estimates [ positive scalar {1e-2} ]
        'FixedPointMaxFlowDistance', 5 ...
);

%         'SimilarityMeasure', 'SSD', ... % [ {SSD} | NCC | CR | MI | NMI ]
%UDiffTol - Fixed point iteration termination tolerance on the maximum sum
%       of squared differences between successive displacement field
%       estimates [ positive scalar {1e-2} ]
%UDiffTol - Fixed point iteration termination tolerance on the maximum sum
%       of squared differences between successive velocity field estimates
%           [ positive scalar {1e-2} ]
%SimMeasTol - Fixed point iteration termination tolerance on the similarity
%       measure [ positive scalar {1e-2} ]
%SimMeasDiffTol - Fixed point iteration termination tolerance on the
%       difference between successive similarity measure estimates
%           [ positive scalar {1e-2} ]
%SimMeasPercentDiffTol - Fixed point iteration termination tolerance on
%       the percentage difference between successive similarity measure
%       estimates [ positive scalar {1e-2} ]
%FixedPointMaxFlowDistance - Maximum distance (in voxels) that the
%       deformation field is allowed to flow during each fixed point
%       iteration [ positive scalar {5.0} ]
%RegridTol - Tolerance on the Jacobian of the deformation field, below which
%       regridding will take place [ positive scalar {0.0025} ]
%Mu - Lame constant for use with elastic or fluid regularizers
%           [ positive scalar {1.0} ]
%Lambda - Lame constant for use with elastic or fluid regularizers
%           [ nonnegative scalar {0.0} ]
    [deformed_image_raw, deformation_field_raw, exit_flag] = npReg(image_2.RawImage, image_1.RawImage, options);
    deformed_image.ChangeRawImage(deformed_image_raw);

    if (exit_flag ~= 1)
        reporting.ShowWarning('PTKGetFluidDeformationField:RegistrationDidNotConverge', 'npReg registration did not converge.', []);
    end

    translation = (image_2.Origin - image_1.Origin).*image_2.VoxelSize;
    deformation_field_raw(:,:,:,1) = deformation_field_raw(:,:,:,1) + translation(1);
    deformation_field_raw(:,:,:,2) = deformation_field_raw(:,:,:,2) + translation(2);
    deformation_field_raw(:,:,:,3) = deformation_field_raw(:,:,:,3) + translation(3);

    deformation_field.ChangeRawImage(deformation_field_raw);
end


function F = cut(varargin)
%CUT Defines a cut constraint
%
% The syntax for CUT is exactly the same as the
% syntax for ordinary constraints.
%
% The difference between a ordinary constraint and
% a cut constraint is that the CUT will not be used
% in the solution of the upper bound problem in a
% global solver, but only in the relaxation for the
% lower problem.

switch nargin
case 0
    F = lmi;
case 1
    F = lmi(varargin{1});
case 2
    F = lmi(varargin{1},varargin{2});
case 3
    F = lmi(varargin{1},varargin{1},varargin{3});
otherwise
end

F = setcutflag(F);


function binary_image = PTKSkeletonise(binary_image, fixed_points_global, reporting)
    % PTKSkeletonise. Performs a skeletonisation on a segmented airway tree.
    %
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    if ~isa(binary_image, 'PTKImage')
        error('Requires a PTKImage as input');
    end

    if ~exist('reporting', 'var')
        reporting = CoreReportingDefault;
    end

    if isdeployed || exist('PTKFastIsSimplePoint') == 3 %#ok<EXIST>
        use_mex_simple_point = true;
    else
        use_mex_simple_point = false;
        warning_message = 'Could not find compiled mex file PTKFastIsSimplePoint. Using a slower version PTKIsSimplePoint instead. Increase program speed by running mex PTKFastIsSimplePoint.cpp in the mex folder.';
        reporting.ShowWarning('PTKSkeletonise:PTKFastIsSimplePointnotFound', warning_message, []);
    end

    fixed_points = binary_image.GlobalToLocalIndices(fixed_points_global);

    % Marks endpoints with 3
    binary_image = binary_image.Copy;
    binary_image.ChangeRawImage(MarkEndpoints(binary_image.RawImage, fixed_points));
    total_number_of_points = sum(binary_image.RawImage(:) > 0);

    binary_image.AddBorder(2);
    direction_vectors = CalculateDirectionVectors;

    raw_image = binary_image.RawImage;

    previous_image = zeros(size(raw_image), 'uint8');

    iteration = 0;

    while ~isequal(previous_image, raw_image)
        previous_image = raw_image;

        iteration = iteration + 1;
        if (iteration > 20)
            if isempty(reporting)
                error('Maximum number of iterations exceeded. This can occur if not all the airway endpoints have been specified correctly.');
            else
                reporting.Error('PTKSkeletonise:MaximumIterationsExceeded', 'Maximum number of iterations exceeded. This can occur if not all the airway endpoints have been specified correctly.');
            end
        end

        % For each of the 6 principal directions
        for direction = [5, 23, 11, 17, 13, 15]

            if ~isempty(reporting)
                number_remaining_points = sum(raw_image(:) > 0);
                progress_value = round(100*(1-number_remaining_points/total_number_of_points));
                reporting.UpdateProgressAndMessage(progress_value, ['Skeletonisation: Iteration ' int2str(iteration)]);
            end

            if ~isempty(reporting)
                if reporting.HasBeenCancelled
                    error('User cancelled');
                end
            end

            direction_vector = direction_vectors(direction,:);
            i = direction_vector(1);
            j = direction_vector(2);
            k = direction_vector(3);

            % Detect border points and get their indices
            [b_i, b_j, b_k] = ind2sub(size(raw_image) - [2 2 2], ...
                find((1 == raw_image(2:end-1, 2:end-1, 2:end-1)) & (0 == raw_image(2+i:end-1+i,2+j:end-1+j,2+k:end-1+k))));
            b_i = b_i + 1; b_j = b_j + 1; b_k = b_k + 1;

            % Iterate through each border point and delete (set to zero) if
            % it is a simple point
            for i = 1 : length(b_i)
                raw_image(b_i(i), b_j(i), b_k(i)) = ~IsPointSimple(raw_image, b_i(i), b_j(i), b_k(i), use_mex_simple_point);
            end
        end
    end
    binary_image.ChangeRawImage(raw_image);
    binary_image.RemoveBorder(2);
end

function is_simple = IsPointSimple(binary_image, i, j, k, use_mex_simple_point)
    if use_mex_simple_point
        % MEX function (fast)
        is_simple = PTKFastIsSimplePoint((binary_image(i-1:i+1, j-1:j+1, k-1:k+1)));
    else
        % Matlab function (slow)
        is_simple = PTKIsSimplePoint(binary_image(i-1:i+1, j-1:j+1, k-1:k+1));
    end
end

function binary_image = MarkEndpoints(binary_image, fixed_points)
    binary_image = int8(binary_image ~= 0);
    binary_image(fixed_points) = 3;
end

function direction_vectors = CalculateDirectionVectors
    [i, j, k] = ind2sub([3 3 3], 1 : 27);
    direction_vectors = [i' - 2, j' - 2, k' - 2];
end


function rgb = yellow

% This returns a predefined color as [red green blue] values
%   red               = [255   0   0]/255;
%   green             = [  0 192   0]/255;
%   blue              = [  0   0 255]/255;
%   magenta           = [255 255   0]/255;
%   cyan              = [  0 255 255]/255;
%   yellow            = [255 255   0]/255;
%   white             = [255 255 255]/255;
%   black             = [  0   0   0]/255;
%
%   skull             = [140  85  85]/255
%   cortex            = [255 213 119]/255;
%   cortex_light      = [199 194 169]/255;
%   cortex_dark       = [100  97  85]/255;
%   skin              = [249 223 192]/255;
%   skin_light        = [249 223 192]/255;
%   skin_medium_light = [225 194 158]/255;
%   skin_medium       = [188 142 106]/255;
%   skin_medium_dark  = [155 102	65]/255;
%   skin_dark         = [ 91  71  61]/255;

rgb = [255 255 0]/255;


report_this_filefun(mfilename('fullpath'));

R = 0.3:1:20;
Tw = 0.1:0.1:4.5;
aa = a;

%maepi = [ -116.5 34.25 1992.48 6 6 7.2 10  ];
M = zeros(length(R),length(Tw))*nan; i1 = 0; i2 = 0;
maepi(1,3) = 1992.48;
an = maepi(1,:);
y = an(1,2),x = an(1,1); z = an(:,7); Te = an(:,3);
%figure_w_normalized_uicontrolunits(map)
axes(hs);
[x,y] = ginput(1)
Te = max(a.Date)
z = 5

l = a.Date < Te;
a = a.subset(l);

dT = max(a.Date) - min(a.Date);
di = sqrt(((a.Longitude-x)*cos(pi/180*y)*111).^2 + ((a.Latitude-y)*111).^2 + (a.Depth-z).^2) ;

N =[];
for i1 = 1:1:length(R)
    ;
    % take first ni points
    %
    l = di <= R(i1);
    b = a.subset(l);      % ne
    for i2 = 1:1:length(Tw)

        l = b(:,3) >= Te-Tw(i2) & b(:,3) < Te;
        Q = length(b(l,3)) ;   % This is the annual rate
        B = length(b(:,1))*Tw(i2)/dT;
        P = poisscdf(Q,B);

        M(i1,i2) = P;

    end
    N =[N ;  length(b)];
    i2 = 0;
end
i1 = 0;

% plot the results
plotqmatri;

a = aa;


classdef RigidJoint < CoordinateFrame
    % A coordinate frame that is rigidly attached to the origin a rigid
    % joint and the child link of the joint. In addition, this class stores
    % the type, rotation axis, child link, and limit of the rigid joints.
    %
    % Copyright (c) 2016-2017, AMBER Lab
    % All right reserved.
    %
    % Redistribution and use in source and binary forms, with or without
    % modification, are permitted only in compliance with the BSD 3-Clause
    % license, see
    % http://www.opensource.org/licenses/bsd-license.php

    properties (SetAccess=protected, GetAccess=public)
        % The type of the rigid joint
        %
        % @note valid joint types include:
        % revolute, prismatic, continuous, fixed.
        %
        % @type char
        Type

        % The rotation axis vector of the rigid joint
        %
        % @note it must be a 3x1 vector.
        %
        % @type rowvec
        Axis

        % The name of the child link
        %
        % @type char
        Child

        % The name of the parent link
        %
        % @type char
        Parent

        % A structure data stores the physical limits of the rigid joints
        %
        % Required fields of limit:
        % effort: the maximum actuator effort @type double
        % lower: the minimum allowable displacement @type double
        % upper: the maximum allowable displacement @type double
        % velocity: the maximum allowable velocity @type double
        %
        % @type struct
        Limit

        % A structure contains information about the actuator for the joint
        %
        % An empty property indicates the joint is not actuated.
        %
        % @type struct
        Actuator

    end

    properties (Hidden, SetAccess=protected, GetAccess=public)

        % The indices of kinematic chains from the base to the current
        % joint
        %
        % @type rowvec
        ChainIndices

        % The twist from the base frame
        %
        % @type rowvec
        Twist

        % The twist pairs of all precedent joints (coordinate frame)
        %
        % @type SymExpression
        TwistPairs
    end
    methods

        function obj = RigidJoint(varargin)
            % The class constructor function
            %
            % Parameters:
            %  varargin: variable nama-value pair input arguments, in detail:
            %    Name: the name of the frame @type char
            %    Reference: the reference frame @type CoordinateFrame
            %    Offset: the offset of the origin @type rowvec
            %    R: the rotation matrix or the Euler angles @type rowvec
            %    Axis: the rotation axis vector @type rowvec
            %    Type: the type of the joints @type char
            %    Child: the child link frame @type RigidLink
            %    Limit: the joint physical limits @type struct

            % consruct the superclass object first
            obj = obj@CoordinateFrame(varargin{:});
            if nargin == 0
                return;
            end
            argin = struct(varargin{:});

            % validate and assign the joint type
            if isfield(argin, 'Type') && ~isempty(argin.Type)
                obj = obj.setType(argin.Type);
            else
                error('The joint type is not defined.');
            end

            % validate and assign the joint axis
            if isfield(argin, 'Axis') && ~isempty(argin.Axis)
                obj = obj.setAxis(argin.Axis);
            elseif strcmp(obj.Type, 'fixed')
                obj = obj.setAxis([0,0,1]);
            else
                error('The joint rotation axis is not defined.');
            end

            % validate and assign the child link
            if isfield(argin, 'Child') && ~isempty(argin.Child)
                obj = obj.setChild(argin.Child);
            else
                error('The child link is not defined.');
            end

            % validate and assign the parent link
            if isfield(argin, 'Parent') && ~isempty(argin.Parent)
                obj = obj.setParent(argin.Parent);
            else
                error('The parent link is not defined.');
            end

            % validate and assign the physical limits
            if isfield(argin, 'Limit') && ~isempty(argin.Limit)
                obj = obj.setLimit(argin.Limit);
            else
                warning('The joint limits are not defined. Using default values.');
                default_limit = struct(...
                    'effort',0,...
                    'lower',-inf,...
                    'upper',inf,...
                    'velocity',inf);
                obj = obj.setLimit(default_limit);
            end

            % validate and assign the actuator info
            if isfield(argin, 'Actuator') && ~isempty(argin.Actuator)
                obj = obj.setActuator(argin.Actuator);
            end
        end

    end

    %% methods defined in external files
    methods
        obj = setAxis(obj, axis);

        obj = setType(obj, type);

        obj = setParent(obj, parent);

        obj = setChild(obj, child);

        obj = setLimit(obj, varargin);

        obj = setActuator(obj, varargin);

        obj = setChainIndices(obj, indices);

        xi = getTwist(obj);

        obj = computeTwist(obj);

        obj = setTwistPairs(obj, dofs, q);
    end
end


function [value,isterminal,direction,eventDesc] = ma_NodeEvents(T,Y, node, bodyInfo, refBody, celBodyData)
    if(strcmpi(T, 'getInfo'))
        value = node;
        isterminal = bodyInfo;
        direction = refBody;
        eventDesc = [];

        return
    end

    if(~isempty(refBody) && bodyInfo.id ~= refBody.id)
        value = [];
        isterminal = [];
        direction = [];
        eventDesc = {};

        return
    end

    rVectECI = Y(1:3);

    topLevelBodyInfo = getTopLevelCentralBody(celBodyData);
    bodyInfoSun = topLevelBodyInfo;
    [rVectECI,vVectECI] = getAbsPositBetweenSpacecraftAndBody(T, rVectECI, bodyInfoSun, bodyInfo, celBodyData);

    [lat, ~, ~] = getLatLongAltFromInertialVect(T, -rVectECI, bodyInfo, vVectECI);

    value = lat;
    isterminal = 1;

    if(strcmpi(node,'asc'))
        direction = 1;
    else
        direction = -1;
    end

    eventDesc = [upper(node), ' Node'];
end


function cascade_data(model, data_year, pca)

% cascade_data(model, data_year, pca)
%
% Compute score statistics for filters and deformation models
% using the model and training data associated with the model
% and dataset year 'data_year'.  If PCA is given a value > 0, then
% the score statistics are computed using a PCA projection of the
% model and training data.
%
% The score statistics are written in to the file
% class_year_cascade_data_pcaK_data_year.inf, which is used
% by cascade_model.m.
%
% model      object detector
% data_year  dataset year as a string (e.g., '2007')
% pca        number of PCA components to project onto (if pca > 0)

setVOCyear = model.year;
globals;
pascal_init;

% if using PCA, project the model
if pca > 0
  load('pca.mat');
  [ignore, model] = projectmodel(model, coeff, pca);
end

% files used by scorestat.cc
pca_str = num2str(pca);
class_year = [model.class '_' model.year];
detfile = [cscdir class_year '_cascade_data_det_' data_year '.inf'];
inffile = [cscdir class_year '_cascade_data_pca' pca_str '_' data_year '.inf'];

% load training images
pos = pascal_data(model.class, true, data_year);
if pca == 0
  % file for recording the detection location and component of each detection
  detfid = fopen(detfile, 'w');
  validinfo = [];
else
  % read the detection location and component used for each detection by the
  % non-PCA model
  pattern = ['%n%n%n' repmat('%n', 1, model.numblocks)];
  detfid = fopen(detfile);
  D = textscan(detfid, '%d %d %d %d %d', 'Delimiter', ' ');
  fclose(detfid);
  validinfo = [D{2} D{3} D{4} D{5}];
end

% reserve an int at the beginning of the file
inffid = fopen(inffile, 'wb');
fwrite(inffid, 0, 'int32');
% compute detections on training images
num = process(model, pos, validinfo, pca, detfid, inffid);
% rewind and write sample size
frewind(inffid);
fwrite(inffid, num, 'int32');
fclose(inffid);
if pca == 0
  fclose(detfid);
end
fprintf('Wrote score statistics for %d examples.\n', num);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Write detection info file and detection feature vector file.
function num = process(model, pos, validinfo, pca, detfid, inffid)

numpos = length(pos);
pixels = model.minsize * model.sbin;
minsize = prod(pixels);

num = 0;
batchsize = 16;
% process positive examples in parallel batches
for j = 1:batchsize:numpos
  % do batches of detections in parallel
  thisbatchsize = batchsize - max(0, (j+batchsize-1) - numpos);
  % data for this batch
  pardata = {};
  parfor k = 1:thisbatchsize
    i = j+k-1;
    fprintf('%s %s: cascade data (PCA=%d): %d/%d', procid(), model.class, pca, i, numpos);
    bbox = [pos(i).x1 pos(i).y1 pos(i).x2 pos(i).y2];
    valid = struct('c',0,'x',0,'y',0,'l',0);
    % skip small examples
    if (bbox(3)-bbox(1)+1)*(bbox(4)-bbox(2)+1) < minsize
      pardata{k}.det = [];
      if pca == 0
        pardata{k}.detinfo = valid;
      end
      fprintf(' (too small)\n');
      continue;
    end
    % get example
    im = imreadx(pos(i));
    [im, bbox] = croppos(im, bbox);
    pyra = featpyramid(im, model);
    if pca > 0
      pyra = projectpyramid(model, pyra);
      valid.c = validinfo(i,1);
      valid.x = validinfo(i,2);
      valid.y = validinfo(i,3);
      valid.l = validinfo(i,4);
    end
    [det, boxes, info] = gdetectvalid(pyra, model, 0, bbox, 0.5, valid);
    pardata{k}.detinfo = getdetinfo(model, info);
    pardata{k}.det = det;
    pardata{k}.boxes = boxes;
    pardata{k}.info = info;
    pardata{k}.pyra = pyra;

    if ~isempty(det)
      fprintf(' (%f)\n', det(end));
      num = num + 1;
      %showboxes(im, det);
    else
      fprintf(' (skip)\n');
    end
  end
  % write data to disk sequentially
  for k = 1:thisbatchsize
    i = j+k-1;
    if ~isempty(pardata{k}.det)
      writescores(pardata{k}.pyra, model, pardata{k}.info, inffid);
    end
    if pca == 0
      % record the detection location and component for this detection
      detinfo = pardata{k}.detinfo;
      fprintf(detfid, '%d %d %d %d %d\n', ...
              i, detinfo.c, detinfo.x, detinfo.y, detinfo.l);
    end
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Retrieve the (x,y,scale) location of the detection in feature
% pyramid coordinates.
function d = getdetinfo(model, info)

d.c = 0;
d.x = 0;
d.y = 0;
d.l = 0;

if ~isempty(info)
  % See: gdetectwrite.m and getdetections.cc
  DET_IND = 2;    % rule index (i.e. component number)
  DET_X   = 3;    % x coord (filter and deformation)
  DET_Y   = 4;    % y coord (filter and deformation)
  DET_L   = 5;    % level (filter)

  ruleind = info(DET_IND, model.start, 1);
  d.c = ruleind;
  d.x = info(DET_X, model.start, 1);
  d.y = info(DET_Y, model.start, 1);
  d.l = info(DET_L, model.start, 1);
end


% Copyright 2011 Zdenek Kalal
%
% This file is part of TLD.
%
% TLD is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% TLD is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with TLD.  If not, see <http://www.gnu.org/licenses/>.

function source = tldInitFirstFrame(tld,source,min_win)

% load the first frame into memory
source.im0  = img_get(source,source.idx(1));

% set the initial bounding box:
% - from file
if source.camera == 0 && exist([source.input '/init.txt'],'file')
    bb = dlmread([source.input '/init.txt']);
    source.bb = bb(:);

    % check
    if isempty(source.bb) || min(bb_size(source.bb)) < min_win
        exit('Error: bounding box is incorrectly defined or too small');
    end
% - by mouse
else
    source.bb = bb_click(tld,source.im0.input);

    % check
    if isempty(source.bb) || min(bb_size(source.bb)) < min_win
       source = [];
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   MoveAbsJ:
%   Moves robot to a particular joint coordinates
%   Example: MoveAbsJ(robot, joint_coord, 'v1000', 'z100', gripper, wobj0);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function MoveAbsJ(joint_coord, speeddata, zonedata, gripper, Wobj)

global configuration robot

fprintf('\nCall to MoveAbsJ');

%obtain current joint coordinates
q_current=robot.q;

%obtain target joint coordinates
q_final= joint_coord(1:robot.DOF)';

%test joint limits for the final values
test_joints(robot, q_final);

speed = obtain_joint_speed(robot, speeddata);
%when fine is specified, the radius is zero, otherwise,
%the radius represents a sphere around the target where the
%movement is changed to the next target
radius = obtain_zone_data(zonedata);

%find time to perform movement
[actual_speed, tmax]=synchronize(q_current, q_final, speed, robot.accelmax);

%in this case, if the total time for the movement is lower than the
%configuration.delta_time time then 10 points are interpolated
if tmax <= configuration.delta_time
    t = [0:tmax/10:tmax]';
else
    %local time for the planning, normal case
    t = [0:configuration.delta_time:tmax]';
end

if radius==0
    %compute a smooth trajectory in q and qd
    %in this case, the final speed is zero
    [q, qd, qdd] = compute_joint_trajectory_indep(robot, q_current, q_final, robot.qd, zeros(1,robot.DOF), t);
else
    %in this case, the final speed is selected as the speed selected by the
    %variable speeddata
    [q, qd, qdd] = compute_joint_trajectory_indep(robot, q_current, q_final, robot.qd, actual_speed, t);
end

T=directkinematic(robot, q_final);
%finds the first joint coordinates that are inside a radius r of the target
%point
index = find_first_in_zone_data(robot, q, T, radius);

%the robot performs the movement until the index found. The coordinates, joint speed and acceleratin
%are stored and used in the planning of the next point
robot.q=q(:,index);
robot.qd=qd(:,index);
robot.qdd=qdd(:,index);

%store all the trajectory for plotting
%the joint trajectories, speeds and acceleration of susequent movements are
%store here
robot.q_vector=[q(:, 1:index)];
robot.qd_vector=[qd(:, 1:index)];
robot.qdd_vector=[qdd(:, 1:index)];

%a global time for the planning is computed.
%in this way, the total trajectory of different movements can be plotted
if length(robot.time)==0
    tend = 0;
else
    tend = robot.time(end);
end
t = t + tend;
%store total time
%robot.time=[robot.time t(1:index)'];
robot.time=[t(1:index)'];

%Test whether there are joints outside mechanical limits
test_joint_limits(robot);

%Plot position, velocity and acceleration
%plot_joint_data(robot);
%Now, animate the robot in 3D
animate(robot, q);

%Plot the trajectory
pp = zeros(3,size(q,2));
for i=1:size(q,2),
    T=directkinematic(robot, q(:,i));
    pp(1:3,i)=T(1:3,4);
end
plot3(pp(1,:),pp(2,:),pp(3,:), 'k', 'LineWidth', 2)


function  MENSRB  = readMENSRB(fid)
%
% <<<<<<<<<< CLASSIFICATION: UNCLASSIFIED >>>>>>>>>>
%
% READMENSRB Create structure for metadata fields.
%   MENSRB = READMENSRB(FID) returns a structure of fields and their
%   associated values.

% Written by Matt Donath, NGA, matthew.b.donath@nga.ic.gov
% References:
%  > MIL-STD-2500A, NATIONAL IMAGERY TRANSMISSION FORMAT VERSION 2.0
%  > MIL-STD-2500C, NATIONAL IMAGERY TRANSMISSION FORMAT VERSION 2.1
%  > STDI-0001,     NATIONAL SUPPORT DATA EXTENSIONS (SDE)(VERSION 1.3/CN2)
%                   FOR THE NATIONAL IMAGERY TRANSMISSION FORMAT (NITF)
%  > STDI-0002,     THE COMPENDIUM OF CONTROLLED EXTENSIONS (CE) FOR THE
%                   NATIONAL IMAGERY TRANSMISSION FORMAT (NITF) VERSION 2.1
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

%% Create the metadata structure for MENSRB.

MENSRB.CETAG = fread(fid,6,'uint8=>char')';
MENSRB.CEL = str2double(fread(fid,5,'uint8=>char')');

MENSRB.ACFT_LOC = fread(fid,25,'uint8=>char')';
MENSRB.ACFT_LOC_ACCY = str2double(fread(fid,6,'uint8=>char')');
MENSRB.ACFT_ALT = str2double(fread(fid,6,'uint8=>char')');
MENSRB.RP_LOC = fread(fid,25,'uint8=>char')';
MENSRB.RP_LOC_ACCY = str2double(fread(fid,6,'uint8=>char')');
MENSRB.RP_ELV = str2double(fread(fid,6,'uint8=>char')');
MENSRB.OF_PC_R = str2double(fread(fid,7,'uint8=>char')');
MENSRB.OF_PC_A = str2double(fread(fid,7,'uint8=>char')');
MENSRB.COSGRZ = str2double(fread(fid,7,'uint8=>char')');
MENSRB.RGCCRP = str2double(fread(fid,7,'uint8=>char')');
MENSRB.RLMAP = fread(fid,1,'uint8=>char')';
MENSRB.RP_ROW = str2double(fread(fid,5,'uint8=>char')');
MENSRB.RP_COL = str2double(fread(fid,5,'uint8=>char')');
MENSRB.C_R_NC = str2double(fread(fid,10,'uint8=>char')');
MENSRB.C_R_EC = str2double(fread(fid,10,'uint8=>char')');
MENSRB.C_R_DC = str2double(fread(fid,10,'uint8=>char')');
MENSRB.C_AZ_NC = str2double(fread(fid,9,'uint8=>char')');
MENSRB.C_AZ_EC = str2double(fread(fid,9,'uint8=>char')');
MENSRB.C_AZ_DC = str2double(fread(fid,9,'uint8=>char')');
MENSRB.C_AL_NC = str2double(fread(fid,9,'uint8=>char')');
MENSRB.C_AL_EC = str2double(fread(fid,9,'uint8=>char')');
MENSRB.C_AL_DC = str2double(fread(fid,9,'uint8=>char')');
MENSRB.TOTAL_TILES_COLS = str2double(fread(fid,3,'uint8=>char')');
MENSRB.TOTAL_TILES_ROWS = str2double(fread(fid,5,'uint8=>char')');

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


function [eeg] = read_ns_eeg(filename, epoch)

% READ_NS_EEG read a NeuroScan 3.x or 4.x EEG File
%
% [eeg] = read_ns_eeg(filename, epoch)
%
%   filename     input Neuroscan .eeg file (version 3.x)
%   epoch        which epoch to read (default is all)
%
% The output data structure eeg has the fields:
%   eeg.data(..)    - epoch signal in uV (size: Nepoch x Nchan x Npnt)
% and
%   eeg.label       - electrode labels
%   eeg.nchan       - number of channels
%   eeg.npnt        - number of samplepoints in ERP waveform
%   eeg.time        - time for each sample
%   eeg.rate        - sample rate (Hz)
%   eeg.xmin        - prestimulus epoch start (e.g., -100 msec)
%   eeg.xmax        - poststimulus epoch end (e.g., 900 msec)
%   eeg.nsweeps     - number of accepted trials/sweeps

% Copyright (C) 2003, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% read the neuroscan header
eeg = read_ns_hdr(filename);

% clear the variance part which is empty anyway
eeg = rmfield(eeg, 'variance');

% create a time axis
eeg.time = linspace(eeg.xmin, eeg.xmax, eeg.npnt);

% open the file and seek towards the place where the raw data is
fid = fopen_or_error(filename,'r','ieee-le');

% the default is to read all epochs
if nargin<2
  epoch = 1:eeg.nsweeps;
end

% determine whether it is 16 or 32 bit data
fseek(fid, 0, 'eof');
header_size = 900 + 75*eeg.nchan;
file_size   = ftell(fid);
sample_size = (file_size-header_size)/(eeg.nchan*eeg.npnt*eeg.nsweeps);
% note that the V4 format can have some extra information at the end of
% the file, causing the sample size to be slightly larger than 2 or 4
if floor(sample_size)==2
  epoch_size = eeg.nchan*eeg.npnt*2 + 13;
  datatype ='int16';
elseif floor(sample_size)==4
  datatype ='int32';
  epoch_size = eeg.nchan*eeg.npnt*4 + 13;
elseif floor(sample_size)>4
  % although this is not to be expected, it might be due to a very large "footer" compared to the data size
  % Olga Sysoeva reported that this extention to the datatype detection fixed it for her (see http://bugzilla.fieldtriptoolbox.org/show_bug.cgi?id=547)
  datatype ='int32';
  epoch_size = eeg.nchan*eeg.npnt*4 + 13;
end

% create empty storage for the data
data = zeros(length(epoch), eeg.nchan, eeg.npnt);
sweep = struct();

for i=1:length(epoch)
  fseek(fid, 900, 'bof');               % skip general header
  fseek(fid, 75*eeg.nchan, 'cof');          % skip channel headers
  status = fseek(fid, (epoch(i)-1)*epoch_size, 'cof');  % skip first epochs
  if status~=0
    ft_error('seek error while reading epoch data');
  end

  % fprintf('reading epoch %d at offset %d\n', epoch(i), ftell(fid));

  % read sweep header
  sweep(i).accept   = fread(fid, 1, 'uchar');
  sweep(i).type     = fread(fid, 1, 'ushort');
  sweep(i).correct  = fread(fid, 1, 'ushort');
  sweep(i).rt       = fread(fid, 1, 'float32');
  sweep(i).response = fread(fid, 1, 'ushort');
  sweep(i).reserved = fread(fid, 1, 'ushort');

  % read raw signal
  raw = fread(fid, eeg.nchan*eeg.npnt, datatype);
  if length(raw)~=eeg.nchan*eeg.npnt
    ft_error('fread error while reading epoch data');
  end
  data(i,:,:) = reshape(raw, [eeg.nchan, eeg.npnt]);

end

% convert raw signal to uV
for chan=1:eeg.nchan
  data(:,chan,:) = (data(:,chan,:) - eeg.baseline(chan)) * eeg.factor(chan);
end

% store the epoch information and data in the output structure
eeg.data = squeeze(data);
eeg.sweep = squeeze(sweep');

fclose(fid);


function GenCrossValidationData()
SPR = 0;

tag = sprintf('.SPR%d', SPR);

para.local.clean_wav_root = ChoosePath4OS({'D:\Data\NoiseData\Libri\LibriSpeech\dev-clean', '/media/xiaoxiong/OS/data1/G/REVERB_Challenge'});
para.local.clean_wav_ext = 'flac';
para.local.rir_wav_root = ChoosePath4OS({'D:\Data\NoiseData\ReverbEstimation\RIR_T60_1ch', '/media/xiaoxiong/OS/data1/G/REVERB_Challenge'});
para.local.rir_wav_ext = 'wav';
para.local.noise_wav_root = ChoosePath4OS({'D:\Data\NoiseData\musan\noise', '/media/xiaoxiong/OS/data1/G/REVERB_Challenge'});
para.local.noise_wav_ext = 'wav';
para.local.useFileName = 1;      % if set to 0, load all training data to memory. otherwise, only load file names.
para.local.parallel_wav_root = ChoosePath4OS({'D:\Data\NoiseData\Libri\LibriSpeech\dev-separation', '/media/xiaoxiong/OS/data1/G/REVERB_Challenge'});
para.local.parallel_wav_root = [para.local.parallel_wav_root tag];
para.singlePrecision = 1;

fs = 16000;
para.IO.DynamicMixture.fs = fs;
para.IO.DynamicMixture.nUtt4Iteration = 100;   % dynamically generate 10000 distorted sentences for each training iteration
para.IO.DynamicMixture.SPR_PDF = 'uniform';      % distribution of the power ratio between the two sources, defined as 10log10(P1/P2),
                                                 % where P1>P2 are the power of the two mixture signals
para.IO.DynamicMixture.SPR_para = [-1 1] * SPR;        % parameters of SPR PDF. If use uniform, it is the lowest and highest SPR allowed.
                                                 % if use normal, it is the mean and variance of SPR.
para.IO.DynamicMixture.addReverb = 0;            % whether to add reverberation to the mixed signal
para.IO.DynamicMixture.addNoise = 0;             % whether to add noise to the mixed signal
para.IO.DynamicMixture.SNR_PDF = 'uniform';      % distribution of SNR, choose [uniform|normal]. SNR is defined as the energy ratio P1/P2,
                                                 % where P1>P2 are the power of the two mixture signals
para.IO.DynamicMixture.SNR_para = [0 20];       % parameters of SNR PDF. If use uniform, it is the lowest and highest SNR allowed.
                                                 % if use normal, it is the mean and variance of SNR.

[base_data, para] = LoadWavRIRNoise_Libri(para, 1);
para.IO.DynamicMixture.inputFeature = para.IO.DynamicDistortion.inputFeature;
para.IO.DynamicMixture.fileReader = para.IO.DynamicDistortion.fileReader;

[data, para] = GenDynamicMixture(base_data, para);

distorted = data(1).data;
clean1 = data(2).data;
clean2 = data(3).data;

my_mkdir([para.local.parallel_wav_root '/mixed']);
my_mkdir([para.local.parallel_wav_root '/clean1']);
my_mkdir([para.local.parallel_wav_root '/clean2']);
for i=1:length(distorted)
    PrintProgress(i, length(distorted), 100);
    curr_distorted = double(distorted{i});
    curr_distorted = curr_distorted / max(abs(curr_distorted));
    distorted_filename = [para.local.parallel_wav_root '/mixed/' sprintf('utt%d.wav', i)];
    audiowrite(distorted_filename, curr_distorted, fs);

    curr_clean = double(clean1{i});
    curr_clean = curr_clean / max(abs(curr_clean));
    clean_filename = [para.local.parallel_wav_root '/clean1/' sprintf('utt%d.wav', i)];
    audiowrite(clean_filename, curr_clean, fs);

    curr_clean = double(clean2{i});
    curr_clean = curr_clean / max(abs(curr_clean));
    clean_filename = [para.local.parallel_wav_root '/clean2/' sprintf('utt%d.wav', i)];
    audiowrite(clean_filename, curr_clean, fs);
end


function organizeExperiments_Radiomics_Old(pathFeatures,pathLR,pathCR,outcomes,timeToEvent,nPatient,nonTextName,fSetNames,paramSEP,baselineSEP,freedomSEP,textType,textName)

startpath = pwd;

permTrain = randperm(nPatient.HGJ + nPatient.CHUS); permTest = randperm(nPatient.HMR + nPatient.CHUM); % Random permutation of instances.

nNonText = numel(nonTextName);
nFset = numel(fSetNames);
nameOutcomes = fieldnames(outcomes.HGJ); nOutcomes = numel(nameOutcomes);
nTextType = numel(textType);

training.nonText = struct; training.text = struct; training.outcomes = struct; training.timeToEvent = struct;
testing.nonText = struct; testing.text = struct; testing.outcomes = struct; testing.timeToEvent = struct;

% Organizing non-texture features
cd(pathFeatures)
train1 = load('nonText_HGJ_GTVtot'); train1 = struct2cell(train1); train1 = train1{1};
train2 = load('nonText_CHUS_GTVtot'); train2 = struct2cell(train2); train2 = train2{1};
test1 = load('nonText_HMR_GTVtot'); test1 = struct2cell(test1); test1 = test1{1};
test2 = load('nonText_CHUM_GTVtot'); test2 = struct2cell(test2); test2 = test2{1};
for i = 1:nNonText
    training.nonText.(nonTextName{i}).Data = [train1.(nonTextName{i}).Data;train2.(nonTextName{i}).Data];
    testing.nonText.(nonTextName{i}).Data = [test1.(nonTextName{i}).Data;test2.(nonTextName{i}).Data];
    training.nonText.(nonTextName{i}).Data = training.nonText.(nonTextName{i}).Data(permTrain);
    testing.nonText.(nonTextName{i}).Data = testing.nonText.(nonTextName{i}).Data(permTest);
end

% Organizing texture features
for f = 1:nFset
    training.text.(fSetNames{f}) = struct; testing.text.(fSetNames{f}) = struct;
    if strcmp(fSetNames{f},'PET') % Using only PET
        train1 = load('text_HGJ_PT_GTVtot'); train1 = struct2cell(train1); train1 = train1{1};
        train2 = load('text_CHUS_PT_GTVtot'); train2 = struct2cell(train2); train2 = train2{1};
        test1 = load('text_HMR_PT_GTVtot'); test1 = struct2cell(test1); test1 = test1{1};
        test2 = load('text_CHUM_PT_GTVtot'); test2 = struct2cell(test2); test2 = test2{1};
        train1 = {train1}; train2 = {train2};
        test1 = {test1}; test2 = {test2};
        training.text.(fSetNames{f}).param = paramSEP;
        training.text.(fSetNames{f}).baseline = baselineSEP;
        training.text.(fSetNames{f}).freedom = freedomSEP;
        training.text.(fSetNames{f}).paramName = {'Scale','Quant.algo','Ng'};
        textCellName = {'PT'};
    elseif strcmp(fSetNames{f},'CT') % Using only CT
        train1 = load('text_HGJ_CT_GTVtot'); train1 = struct2cell(train1); train1 = train1{1};
        train2 = load('text_CHUS_CT_GTVtot'); train2 = struct2cell(train2); train2 = train2{1};
        test1 = load('text_HMR_CT_GTVtot'); test1 = struct2cell(test1); test1 = test1{1};
        test2 = load('text_CHUM_CT_GTVtot'); test2 = struct2cell(test2); test2 = test2{1};
        train1 = {train1}; train2 = {train2};
        test1 = {test1}; test2 = {test2};
        training.text.(fSetNames{f}).param = paramSEP;
        training.text.(fSetNames{f}).baseline = baselineSEP;
        training.text.(fSetNames{f}).freedom = freedomSEP;
        training.text.(fSetNames{f}).paramName = {'Scale','Quant.algo','Ng'};
        textCellName = {'CT'};
    end

    for i = 1:numel(train1)
        sizeParam = size(train1{1});
        training.text.(fSetNames{f}).(textCellName{i}) = cell(sizeParam);
        testing.text.(fSetNames{f}).(textCellName{i}) = cell(sizeParam);
        for n = 1:numel(train1{1})
            for type = 1:nTextType
                for text = 1:numel(textName{type})
                    training.text.(fSetNames{f}).(textCellName{i}){n}.(textType{type}).(textName{type}{text}).Data = [train1{i}{n}.(textType{type}).(textName{type}{text}).Data;train2{i}{n}.(textType{type}).(textName{type}{text}).Data];
                    testing.text.(fSetNames{f}).(textCellName{i}){n}.(textType{type}).(textName{type}{text}).Data = [test1{i}{n}.(textType{type}).(textName{type}{text}).Data;test2{i}{n}.(textType{type}).(textName{type}{text}).Data];
                    training.text.(fSetNames{f}).(textCellName{i}){n}.(textType{type}).(textName{type}{text}).Data = training.text.(fSetNames{f}).(textCellName{i}){n}.(textType{type}).(textName{type}{text}).Data(permTrain);
                    testing.text.(fSetNames{f}).(textCellName{i}){n}.(textType{type}).(textName{type}{text}).Data = testing.text.(fSetNames{f}).(textCellName{i}){n}.(textType{type}).(textName{type}{text}).Data(permTest);
                end
            end
        end
    end
end

% Organizing outcomes
for o = 1:nOutcomes
    training.outcomes.(nameOutcomes{o}) = [outcomes.HGJ.(nameOutcomes{o});outcomes.CHUS.(nameOutcomes{o})];
    testing.outcomes.(nameOutcomes{o}) = [outcomes.HMR.(nameOutcomes{o});outcomes.CHUM.(nameOutcomes{o})];
    training.timeToEvent.(nameOutcomes{o}) = [timeToEvent.HGJ.(nameOutcomes{o});timeToEvent.CHUS.(nameOutcomes{o})];
    testing.timeToEvent.(nameOutcomes{o}) = [timeToEvent.HMR.(nameOutcomes{o});timeToEvent.CHUM.(nameOutcomes{o})];
    training.outcomes.(nameOutcomes{o}) = training.outcomes.(nameOutcomes{o})(permTrain);
    testing.outcomes.(nameOutcomes{o}) = testing.outcomes.(nameOutcomes{o})(permTest);
    training.timeToEvent.(nameOutcomes{o}) = training.timeToEvent.(nameOutcomes{o})(permTrain);
    testing.timeToEvent.(nameOutcomes{o}) = testing.timeToEvent.(nameOutcomes{o})(permTest);
end

% Re-arranging by outcomes (same features for all outcomes here)
tempTraining = training; tempTesting = testing; clear training testing
for o = 1:nOutcomes
    training.(nameOutcomes{o}).nonText = tempTraining.nonText;
    training.(nameOutcomes{o}).text = tempTraining.text;
    training.(nameOutcomes{o}).outcome = tempTraining.outcomes.(nameOutcomes{o});
    training.(nameOutcomes{o}).timeToEvent = tempTraining.timeToEvent.(nameOutcomes{o});
    testing.(nameOutcomes{o}).nonText = tempTesting.nonText;
    testing.(nameOutcomes{o}).text = tempTesting.text;
    testing.(nameOutcomes{o}).outcome = tempTesting.outcomes.(nameOutcomes{o});
    testing.(nameOutcomes{o}).timeToEvent = tempTesting.timeToEvent.(nameOutcomes{o});
end

cd(pathLR), save('training','training'), save('testing','testing'), save('permTrain','permTrain'), save('permTest','permTest')
cd(pathCR), save('training','training'), save('testing','testing'), save('permTrain','permTrain'), save('permTest','permTest')

cd(startpath)
end


function hash = xVOChash_init(strs)
% From the PASCAL VOC 2011 devkit

hsize=4999;
hash.key=cell(hsize,1);
hash.val=cell(hsize,1);

for i=1:numel(strs)
    s=strs{i};
    h=mod(str2double(s([4 6:end])),hsize)+1;
    j=numel(hash.key{h})+1;
    hash.key{h}{j}=strs{i};
    hash.val{h}(j)=i;
end


function engine = jtree_mnet_inf_engine(model, varargin)
% JTREE_MNET_INF_ENGINE Junction tree inference engine for Markov nets
% engine = jtree_inf_engine(mnet, ...)
%

% set default params
N = length(mnet.graph);
root = N;

engine = init_fields;
engine = class(engine, 'jtree_mnet_inf_engine', inf_engine(bnet));

onodes = bnet.observed;
if is_mnet(bnet)
  MG = bnet.graph;
else
  error('should be a mnet')
end

%[engine.jtree, dummy, engine.cliques, B, w, elim_order, moral_edges, fill_in_edges, strong] = ...
%    dag_to_jtree(bnet, onodes, stages, clusters);

porder = determine_elim_constraints(bnet, onodes);
strong = ~isempty(porder);
ns = bnet.node_sizes(:);
ns(onodes) = 1; % observed nodes have only 1 possible value
[engine.jtree, root2, engine.cliques, B, w] = ...
    graph_to_jtree(MG, ns, porder, stages, clusters);

engine.cliques_bitv = B;
engine.clique_weight = w;
C = length(engine.cliques);
engine.clpot = cell(1,C);

% Compute the separators between connected cliques.
[is,js] = find(engine.jtree > 0);
engine.separator = cell(C,C);
for k=1:length(is)
  i = is(k); j = js(k);
  engine.separator{i,j} = find(B(i,:) & B(j,:)); % intersect(cliques{i}, cliques{j});
end

% A node can be a member of many cliques, but is assigned to exactly one, to avoid
% double-counting its CPD. We assign node i to clique c if c is the "lightest" clique that
% contains i's family, so it can accomodate its CPD.

engine.clq_ass_to_node = zeros(1, N);
for i=1:N
  %c = clq_containing_nodes(engine, family(bnet.dag, i));
  clqs_containing_family = find(all(B(:,family(bnet.dag, i)), 2)); % all selected columns must be 1
  c = clqs_containing_family(argmin(w(clqs_containing_family)));
  engine.clq_ass_to_node(i) = c;
end

% Make the jtree rooted, so there is a fixed message passing order.
if strong
  % the last clique is guaranteed to be a strong root
  engine.root_clq = length(engine.cliques);
else
  % jtree_dbn_inf_engine requires the root to contain the interface.
  % This may conflict with the strong root requirement! *********** BUG *************
  engine.root_clq = clq_containing_nodes(engine, root);
  if engine.root_clq <= 0
    error(['no clique contains ' num2str(root)]);
  end
end

[engine.jtree, engine.preorder, engine.postorder] = mk_rooted_tree(engine.jtree, engine.root_clq);

% collect
engine.postorder_parents = cell(1,length(engine.postorder));
for n=engine.postorder(:)'
  engine.postorder_parents{n} = parents(engine.jtree, n);
end
% distribute
engine.preorder_children = cell(1,length(engine.preorder));
for n=engine.preorder(:)'
  engine.preorder_children{n} = children(engine.jtree, n);
end

%%%%%%%%

function engine = init_fields()

engine.jtree = [];
engine.cliques = [];
engine.separator = [];
engine.cliques_bitv = [];
engine.clique_weight = [];
engine.clpot = [];
engine.clq_ass_to_node = [];
engine.root_clq = [];
engine.preorder = [];
engine.postorder = [];
engine.preorder_children = [];
engine.postorder_parents = [];
engine.maximize = [];
engine.evidence = [];


function [clusLengths,biggestEvent,mbg,bg,clustnumbers] = funBuildclu(mycat, biggestEvent, clus, mbg)
% FUNBUILDCLU builds cluster out out of information stored in clus
%
%  FUNBUILDCLU(mycat, biggestEvent, clus, mbg)
%
% mycat : catalog
% clus: size of catalog, containing cluster index numbers
% biggestEvent: size nClusters
% mbg: max mag in cluster
% bg : size nClusters
%
    %  originally, "mycat" was "newcat"
    % A.Allmann

% modified by C Reyes 2017

%global mycat clus mbg k1 clust clustnumbers
%global bgevent bg

 % count # events in each cluster
clusLengths = histcounts(clus,'BinLimits',[1 max(clus)], 'BinMethod','integers');

% cull any clusters that don't have events
empty_clusters = clusLengths == 0;      %numbers of clusters that are empty  eg. [1:9 2:0 3: 8 4:0] ->  [0 1 0 1]
clusLengths(empty_clusters)=[];
biggestEvent(empty_clusters)=[];
mbg(empty_clusters)=[];

bg = biggestEvent;
biggestEvent = mycat.subset(bg); %biggest event in a cluster(if more than one,take first)

clustnumbers=(1:numel(clusLengths));    % stores numbers of clusters


% Convert bin-epoched EEG to a bin-organized EEG structure
% may be useful for subsequent decoding scripts
% axs 2020
%
% Inputs:
%    EEG - an EEG set, which must have been processed with ERPLAB, with
%         valid EventList, and be bin-epoched EEG data
%    write_file_path - optional full path to write file to, string
%
% Outputs:
%  binorgEEG - reorganisation of the EEG data, around the ERPLAB bins
%              with the data in binorgEEG.binwise_data(bin_n).data(elec,timepoints,trial_n)
function binorgEEG = binepEEG_to_binorgEEG(EEG, write_file_path)

dim_data = numel(size(EEG.data));
if dim_data ~= 3
    error('binepEEG_to_binorgEEG works on bin-epoched EEG. Please ensure data is epoched (not continuous) and has bin labels');
end

% Prepare info for averager call
artif = 1; stderror = 1; excbound = 1;

% Call ERP Averager subfunction, populating the epoch_list

[ERP2, EVENTLISTi, countbiORI, countbinINV, countbinOK, countflags, workfname,epoch_list] = averager(EEG, artif, stderror, excbound);

% We now have epoch_list, which has a list of 'good' bepoch indecies to use
% epoch_list(1).good_bep_indx(:) has those trials for bin 1
% epoch_list(2).good_bep_indx(:) has those trials for bin 2, etc
nbin = ERP2.nbin;

% Prepare binorg data structure
for bin = 1:nbin

    binorgEEG.binwise_data(bin).data = EEG.data(:,:,epoch_list(bin).good_bep_indx);
    binorgEEG.n_trials_this_bin(bin) = numel(epoch_list(bin).good_bep_indx);

end

% Now, the data from each trial is saved, reorganized by bin, such that:
% for bin X
%  binorgEEG.binwise_data(X).data(:,:,1) gives all elecs and all
%  timepoints for the 1st trial of bin X

if ischar(write_file_path)
    % If a string of a full write file path is provided in arg, save it there
    save(write_file_path,'binorgEEG')

elseif write_file_path == 1
    % UI file pick when write_file_path == 1
    [file_name, file_path] = uiputfile('*.mat','Please pick a path to save the Bin-Organized EEG data');
    write_file_path = [file_path file_name];
    save(write_file_path,'binorgEEG');
end


for model_index=0:0
    caffe.reset_all;
    net=caffe.get_net(param.test_net_file,'test');
%     net.copy_from(strcat(param.save_model_file,num2str(split_index),'/',param.save_model_name,'_',num2str(65500+model_index*2000),'.caffemodel'));
   net.copy_from(param.fintune_model);
    tic;
    fin=fopen(param.result_save_file,'a');
    str=strcat(param.save_model_file,num2str(split_index),'/',param.save_model_name,'_',num2str(65500+model_index*2000),'.caffemodel');
    fprintf(fin,'model:%s\n',str);
    fprintf('testing model:%s\n',str);
    fclose(fin);
    net.blobs('data').reshape([224 224 3 param.test_batch_size]);
%     % contrast fix score
%     net.blobs('sig').reshape([1 1 3 param.test_batch_size]);
%     batch_sig=ones(1,1,3,param.test_batch_size,'single');
%     net.blobs('sig').set_data(batch_sig);
%     %
    load(strcat(param.data_path,param.testdata_filename,num2str(split_index),'/test_data.mat'));
    test_script_for_LPW;
    toc;
end


function  [fig_handle,tot_image,image_XYYZ,image_YX] = DrawCellsOnAnat(I,ax)
if exist('ax','var') && isobject(ax)
       axes(ax);
       fig_handle = gcf;
else
    fig_handle = figure('Position',[600,50,458.5,608],'color',[1 1 1],...
        'Name',['Fish#' num2str(I.i_fish)]);
    set(gca,'Position',[0.01, 0.01, 0.98, 0.98]); % right ~subplot
end

%% load from Input struct that is generated by 'LoadCurrentFishForAnatPlot.m'
names = fieldnames(I); % cell of strings
for i = 1:length(names),
    eval([names{i} ' = I.',names{i},';']);
end

if ~isRefAnat, % raw images
    k_zres_ratio = 20; % 8/0.406 = 19.704
    radius_xy = 5;%7;
    width_z = 10;
    thickness_z = 1;
else % registered to ZBrain
    k_zres_ratio = 2.5; % 2/0.798 = 2.506
    radius_xy = 3;
    width_z = 5;
    thickness_z = 3;
    z_range_ventral = 40;% round(min(CellXYZ(:,3))*k_zres_ratio);
end

%% Setup
[s1,s2] = size(cIX);
if s2>s1,
    cIX = cIX';
end
[s1,s2] = size(gIX);
if s2>s1,
    gIX = gIX';
end

% down-sample
if ~isPopout,
    displaymax = 8000;
    if length(cIX) > displaymax,
        skip = round(length(cIX)/displaymax);
        cIX = cIX(1:skip:end,:);
        gIX = gIX(1:skip:end,:);
    end
end

% set up anatomical images
% anat_YX = zeros(size(anat_yx));
% anat_YZ = zeros(size(anat_yz));
% anat_ZX = zeros(size(anat_zx));
anat_YX = anat_yx/4;
anat_YZ = anat_yz/4;
anat_ZX = anat_zx/4;
anat_ZX = flipud(anat_ZX);
dimv_yx = size(anat_YX);
dimv_yz = size(anat_YZ);
dimv_zx = size(anat_ZX);

% adjust size for YZ and ZX view, and combine all 3 view into total-image
% (including white borders as indicated in dim_totimage)

anat_yz2=zeros(dimv_yz(1),dimv_yz(2)*k_zres_ratio,3);
anat_zx2=zeros(dimv_zx(1)*k_zres_ratio,dimv_zx(2),3);

% Show cropped version for normalized fish
% (only for plotting for now, because drawing on GUI relies on the true
% coordinates)

if isRefAnat && isPopout,
    % crop lengthwise (most of tail), and scale k_zres
    y_range = 81:980;%81:990;% 81:1104; % max 1406
    z_range = z_range_ventral:345; % max 138*k_zres
    dimv_yx3 = size(anat_YX(y_range,:,:));
    dimv_yz3 = [length(y_range),length(z_range),3];
    dimv_zx3 = [length(z_range),size(anat_YX,2),3];

    dim_totimage = [dimv_yx3(1)+dimv_zx3(1)+10,dimv_yx3(2)+dimv_yz3(2)+10,3];
else
    dim_totimage = [dimv_yx(1)+dimv_zx(1)*k_zres_ratio+10,dimv_yx(2)+dimv_yz(2)*k_zres_ratio+10,3];
end
tot_image=ones(dim_totimage);

% create masks (~linearized indices), to draw cells on image (later)
circlemaskIX = MakeCircularMask(radius_xy,dimv_yx);
yzmaskIX = MakeSquareMask(width_z,thickness_z,dimv_yz);
zxmaskIX = MakeSquareMask(thickness_z,width_z,dimv_zx);

%% Draw each cell on map (all 3 views)
% main loop
cIX_abs = absIX(cIX);
M_xyz = CellXYZ(cIX_abs,:);

if ~isempty(cIX)
    anat_YX = DrawMasksInRGB(anat_YX,M_xyz(:,[1,2]),circlemaskIX,clrmap,gIX,clr_alpha);
    anat_YZ = DrawMasksInRGB(anat_YZ,M_xyz(:,[1,3]),yzmaskIX,clrmap,gIX,clr_alpha/2);
    anat_ZX = DrawMasksInRGB(anat_ZX,M_xyz(:,[3,2]),zxmaskIX,clrmap,gIX,clr_alpha/2);
end

%% Draw masks on anat projections
if isShowMasks,
    nMasks = length(Msk_IDs);
    % set params
    rng(1);
    %     cmap2 = rand(nMasks,3);
    cmap2 = hsv(nMasks);%jet(nMasks);
    outline_radius = 3;
    msk_alpha = 0.1;%0.15 %0.25 for all projections
    white_alpha = 0.15;%0.25 % 0 for all projections
    for i = 1:nMasks,
        clr = cmap2(i,:);
        % get masks from database
        if Msk_IDs(i)==0,
            msk = full(newMask);
        else
            msk = full(MASKs.MaskDatabase(:,Msk_IDs(i)));
        end
        mask_3D = reshape(msk, [MASKs.height, MASKs.width, MASKs.Zs]);

        % Y-X
        masks_XY = max(mask_3D,[],3);
        masks_fit = logical(imresize(masks_XY,[size(anat_yx,1),size(anat_yx,2)]));
        if isShowMskOutline,
            masks_fit = imdilate(edge(masks_fit), strel('disk',outline_radius));
        end
        anat_YX = DrawMasksInRGB(anat_YX,[],masks_fit,clr,1,msk_alpha,white_alpha);

        % Y-Z
        masks_YZ = squeeze(max(mask_3D,[],2));
        masks_fit = logical(imresize(masks_YZ,[size(anat_yz,1),size(anat_yz,2)]));
        if isShowMskOutline,
            masks_fit = imdilate(edge(masks_fit), ones(5,2));%strel('line',outline_radius,90));
        end
        anat_YZ = DrawMasksInRGB(anat_YZ,[],masks_fit,clr,1,msk_alpha,white_alpha);

        % Z-X
        masks_ZX = squeeze(max(mask_3D,[],1))';  % notice the transpose
        masks_fit = logical(imresize(masks_ZX,[size(anat_zx,1),size(anat_zx,2)]));
        if isShowMskOutline,
            masks_fit = imdilate(edge(masks_fit), ones(2,5));%strel('line',outline_radius,0));
        end
        anat_ZX = DrawMasksInRGB(anat_ZX,[],masks_fit,clr,1,msk_alpha,white_alpha);
    end
end

%% rescale (low-res) z dimension
for k=1:3
    anat_yz2(:,:,k) = imresize(anat_YZ(:,:,k), [dimv_yz(1), dimv_yz(2)*k_zres_ratio],'nearest');
    anat_zx2(:,:,k) = imresize(anat_ZX(:,:,k), [dimv_zx(1)*k_zres_ratio, dimv_zx(2)],'nearest');
end

% Draw scale bar
if isRefAnat
    if ~isPopout,
        y_range = 1:size(anat_YX,1);
    end
    k_um = 0.798; % ZBrain: x/y/z = 0.798/0.798/2um
    scalebar_len = round(50/k_um);
    scalebar_x_range = (600-scalebar_len+1):600;
    scalebar_y = y_range(end)-30-y_range(1)+1;
    scalebar_y_range = (scalebar_y-3):(scalebar_y+3);

    image_YX = anat_YX(y_range,:,:);
    image_YX(scalebar_y_range,scalebar_x_range,:) = 1;
else
    k_um = 0.406; % xy_res
    scalebar_len = round(50/k_um);

    x_end = size(anat_YX,2)-30;
    scalebar_x_range = (x_end-scalebar_len+1):x_end;
    scalebar_y = size(anat_YX,1)-30+1;
    scalebar_y_range = (scalebar_y-3):(scalebar_y+3);

    image_YX = anat_YX;
    image_YX(scalebar_y_range,scalebar_x_range,:) = 1;
end

% compile 3 projections to image panel
if isRefAnat && isPopout,
    tot_image(dimv_zx3(1)+11:end,1:dimv_yx3(2),:) = image_YX;%anat_YX(y_range,:,:);
    tot_image(dimv_zx3(1)+11:end,dimv_yx3(2)+11:end,:) = anat_yz2(y_range,z_range,:);
    tot_image(1:dimv_zx3(1),1:dimv_zx3(2),:) = flipud(anat_zx2(z_range,:,:));
    image_XYYZ = tot_image(dimv_zx3(1)+11:end,1:end,:);
else
    tot_image(dimv_zx(1)*k_zres_ratio+11:end,1:dimv_yx(2),:) = image_YX;
    tot_image(dimv_zx(1)*k_zres_ratio+11:end,dimv_yx(2)+11:end,:) = anat_yz2;
    tot_image(1:dimv_zx(1)*k_zres_ratio,1:dimv_zx(2),:) = flipud(anat_zx2);
    image_XYYZ = tot_image(dimv_zx(1)*k_zres_ratio+11:end,1:end,:);
end

% OPTIONAL BRIGHTENING
% tot_image = tot_image*2;

tot_image(tot_image(:)>1) = 1;
tot_image(tot_image(:)<0) = 0;

image(image_XYYZ); % image(tot_image);
axis image;axis off

set(gcf,'Color','w');

end


classdef TextDetectorCNN < handle
    %TEXTDETECTORCNN  Class providing functionality of text detection
    %
    % This class provides the functionality of text bounding box detection
    % (finds bounding boxes of text words given an input image).
    %
    % This class uses OpenCV dnn module to load pre-trained model described in
    % [LiaoSBWL17].
    % The original repository with the modified SSD Caffe version:
    % [GitHub](https://github.com/MhLiao/TextBoxes).
    %
    % Model (90.68 MB) can be downloaded from
    % [DropBox](https://www.dropbox.com/s/g8pjzv2de9gty8g/TextBoxes_icdar13.caffemodel?dl=0).
    %
    % Modified `.prototxt` file with the model description can be obtained from
    % [GitHub](https://github.com/opencv/opencv_contrib/raw/3.4.1/modules/text/samples/textbox.prototxt).
    %
    % ## References
    % [LiaoSBWL17]:
    % > Minghui Liao, Baoguang Shi, Xiang Bai, Xinggang Wang, and Wenyu Liu.
    % > "Textboxes: A fast text detector with a single deep neural network".
    % > In AAAI, 2017.
    %
    % See also: cv.TextDetectorCNN.TextDetectorCNN, cv.Net
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    %% TextDetectorCNN
    methods
        function this = TextDetectorCNN(varargin)
            %TEXTDETECTORCNN  Creates an instance using the provided parameters
            %
            %     obj = cv.TextDetectorCNN(modelArchFilename, modelWeightsFilename)
            %     obj = cv.TextDetectorCNN(..., 'OptionName', optionValue, ...)
            %
            % ## Input
            % * __modelArchFilename__ path to the prototxt file describing the
            %   classifiers architecture (the `.prototxt` file).
            % * __modelWeightsFilename__ path to the file containing the
            %   pretrained weights of the model in caffe-binary form
            %   (the `.caffemodel` file).
            %
            % ## Options
            % * __DetectionSizes__ a list of sizes for multiscale detection.
            %   The values
            %   `{[300,300], [700,500], [700,300], [700,700], [1600,1600]}`
            %   are recommended in [LiaoSBWL17] to achieve the best quality.
            %   default `{[300,300]}`.
            %
            % See also: cv.TextDetectorCNN
            %
            this.id = TextDetectorCNN_(0, 'new', varargin{:});
        end

        function delete(this)
            %DELETE  Destructor
            %
            %     obj.delete()
            %
            % See also: cv.TextDetectorCNN
            %
            if isempty(this.id), return; end
            TextDetectorCNN_(this.id, 'delete');
        end
    end

    %% TextDetector
    methods
        function [bbox, conf] = detect(this, img)
            %DETECT  Detect text inside an image
            %
            %     [bbox, conf] = obj.detect(img)
            %
            % ## Input
            % * __img__ an image to process, expected to be a 8-bit color
            %   image of any size.
            %
            % ## Output
            % * __bbox__ a vector of rectangles of the detected word bounding
            %   boxes.
            % * __conf__ a vector of float of the confidences the classifier
            %   has for the corresponding bounding boxes.
            %
            % See also: cv.TextDetectorCNN.TextDetectorCNN
            %
            [bbox, conf] = TextDetectorCNN_(this.id, 'detect', img);
        end
    end

end


function result = analyzePathDeviationDetection(debug, PerformanceSpec)

% Author: Alexander Wischnewski     Date: 01-07-2019
%
% Description:
%   scans the debug data and ground truth whether the path deviation
%   detection works as specified
%
% Input:
%   debug:          Debug structure of the vehicle
%   PerformanceSpec Structure with fields:
%       HighDeviationLat_m
%       VeryHighDeviationLat_m
%       HighDeviationHeading_rad
%       VeryHighDeviationHeading_rad
%       TimeSpec_s
%
% Outputs:
%   result:       1 if valid according to spec, 0 otherwise

result = 1;

disp(' ');
disp('------------------------------');
disp('Check High Path Deviation Detection: ');
disp('------------------------------');
idxFailure = find(abs(debug.debug_mvdc_path_matching_debug_PathPos_d_m.Data) >= PerformanceSpec.HighDeviationLat_m | ...
    abs(debug.debug_mvdc_path_matching_debug_PathPos_psi_rad.Data) >= PerformanceSpec.HighDeviationHeading_rad, 1);
if isempty(idxFailure)
    disp('The vehicle did not deviate enough from the path for high path deviation detection.');
else
    % load time where failure occured
    tFailure = debug.debug_mloc_statemachine_debug_TUMVehicleState.Time(idxFailure);
    % find time where vehicle has gone to emergency
    idxDetect = find(uint16(debug.debug_mloc_statemachine_debug_TUMVehicleState.Data) == 50, 1);
    tFailureDetected_s = debug.debug_mloc_statemachine_debug_TUMVehicleState.Time(idxDetect);
    disp(['The failure was detected within ' num2str(tFailureDetected_s-tFailure) 's.']);
    if(tFailureDetected_s - tFailure > PerformanceSpec.TimeSpec_s)
        disp('The failure was not detected within the specified time window');
        result = 0;
        return
    end
    disp('The high path deviation failure was detected within the specified time window');
end

disp(' ');
disp('------------------------------');
disp('Check Very High Path Deviation Detection: ');
disp('------------------------------');
idxFailure = find(abs(debug.debug_mvdc_path_matching_debug_PathPos_d_m.Data) >= PerformanceSpec.VeryHighDeviationLat_m | ...
    abs(debug.debug_mvdc_path_matching_debug_PathPos_psi_rad.Data) >= PerformanceSpec.VeryHighDeviationHeading_rad, 1);
if isempty(idxFailure)
    disp('The vehicle did not deviate enough from the path for very high path deviation detection.');
else
    % load time where failure occured
    tFailure = debug.debug_mloc_statemachine_debug_TUMVehicleState.Time(idxFailure);
    % find time where vehicle has gone to emergency
    idxDetect = find(uint16(debug.debug_mloc_statemachine_debug_TUMVehicleState.Data) == 60, 1);
    tFailureDetected_s = debug.debug_mloc_statemachine_debug_TUMVehicleState.Time(idxDetect);
    disp(['The failure was detected within ' num2str(tFailureDetected_s-tFailure) 's.']);
    if((tFailureDetected_s - tFailure) > PerformanceSpec.TimeSpec_s)
        disp('The failure was not detected within the specified time window');
        result = 0;
        return
    end
    disp('The very high path deviation failure was detected within the specified time window');
end

disp(' ');
disp('All checks passed.');


function [data,eid,name] = DBGetValues(query,direction)

%DBGetValues - Group values for all variables that match given criteria.
%
%  Concatenate individual values for all variables matching the optional query.
%  Values must be scalars, vectors or matrices. Concatenation can be performed
%  horizontally or vertically. Matrices can also be concatenated along the
%  third dimension (z). By default, values of size MxN are concatenated
%  vertically if M<=N, or horizontally otherwise.
%
%  USAGE
%
%    [data,eid,name] = DBGetValues(query,direction)
%
%    query          optional list query (WHERE clause; see Example)
%    direction      optional concatenation direction ('h', 'v' or 'z')
%
%  OUTPUT
%
%    data           concatenation of individual values
%    eid            experiment ID (identifier string)
%    name           descriptive name (identifier string)
%
%  EXAMPLE
%
%    In this example, place cells were recorded on successive days while the
%    animal explored a maze, then during sleep. Firing fields were stored in a
%    database using eids like '20120213-Maze-(1,2)' (where 1,2 corresponds to
%    tetrode 1, cluster 2) and named 'FiringFields'. Mean firing rates during
%    sleep were named 'MeanRate'.
%
%    Get all firing fields (for details on query syntax, see an SQL manual):
%
%    [fields,eid1] = DBGetValues('eid like "%Maze%" and name="FiringField"');
%
%    Get all firing rates during sleep:
%
%    [rates,eid2] = DBGetValues('eid like "%Sleep%" and name="MeanRate"');
%
%  SEE
%
%    See also DBMatchValues, DBGetVariables, DBAddVariable, DBGetFigures.
%

% Copyright (C) 2007-2013 by Michaël Zugaro
%
% 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.

% Make sure MyM is installed and functional
CheckMyM;

data = [];
eid = {};
name = {};

% Parameters
if nargin < 1,
	query = '';
	direction = [];
elseif nargin == 1,
	if strcmp(lower(query),'h'),
		query = '';
		direction = 'h';
	elseif strcmp(lower(query),'v'),
		query = '';
		direction = 'v';
	elseif strcmp(lower(query),'z'),
		query = '';
		direction = 'z';
	else
		direction = [];
	end
end

% Query database
results = DBGetVariables(query,'output','full');
if isempty(results.v), return; end
eid = results.eid;
name = results.name;

first = results.v{1};
if ~isnumeric(first),
	error('Values are not numerical and cannot be concatenated (type ''help DBGetValues'' for details).');
end

% Automatic concatenation direction
if isempty(direction),
	if size(first,1) <= size(first,2),
		direction = 'v';
	else
		direction = 'h';
	end
end

% Concatenation

% Determine sizes of retrieved values (arrays)
sizes = cellfun(@size,results.v,'UniformOutput',false);
% Make sure they all have the same number of dimensions
nDims = cellfun(@length,sizes);
if any(nDims~=nDims(1)),
	error('All values should have the same number of dimensions (type ''help DBGetValues'' for details).');
end
% Determine 'circumscribed' array size
s = max(unique(vertcat(sizes{:}),'rows'),[],1);
% Extend all arrays
results.v = cellfun(@(x) ExtendArray(x,s),results.v,'uniformoutput',false);
% Concatenate in the appropriate direction
if strcmp(direction,'v'),
	data = vertcat(results.v{:});
elseif strcmp(direction,'h'),
	data = [results.v{:}];
else
	data = cat(3,results.v{:});
end


%%*****************************************************************************
%% sqlpsummary: print summary
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************************

function  sqlpsummary(info,ttime,infeas_org,printlevel)

iter        = info.iter;
obj         = info.obj;
gap         = info.gap;
relgap      = info.relgap;
prim_infeas = info.pinfeas;
dual_infeas = info.dinfeas;
termcode    = info.termcode;
reldist     = info.reldist;
resid       = info.resid;
dimacs      = info.dimacs;
totaltime   = info.cputime;
%%
preproctime = ttime.preproc; pcholtime = ttime.pchol; dcholtime = ttime.dchol;
predtime  = ttime.pred; pstep_predtime = ttime.pred_pstep; dstep_predtime = ttime.pred_pstep;
corrtime  = ttime.corr; pstep_corrtime = ttime.corr_pstep; dstep_corrtime = ttime.corr_dstep;
misctime  = ttime.misc;
%%
if (printlevel >= 2)
    fprintf('\n------------------------------------------------');
    fprintf('-------------------\n');
    fprintf(' number of iterations   = %2.0f\n',iter);
end
if (termcode <= 0)
    if (printlevel >=2)
        fprintf(' primal objective value = %- 9.8e\n',obj(1));
        fprintf(' dual   objective value = %- 9.8e\n',obj(2));
        fprintf(' gap := trace(XZ)       = %3.2e\n',gap);
        fprintf(' relative gap           = %3.2e\n',relgap);
        fprintf(' actual relative gap    = %3.2e\n',-diff(obj)/(1+sum(abs(obj))));
        if norm(infeas_org)
            fprintf(' rel. primal infeas (scaled problem)   = %3.2e\n',prim_infeas);
            fprintf(' rel. dual     "        "       "      = %3.2e\n',dual_infeas);
            fprintf(' rel. primal infeas (unscaled problem) = %3.2e\n',infeas_org(1));
            fprintf(' rel. dual     "        "       "      = %3.2e\n',infeas_org(2));
        else
            fprintf(' rel. primal infeas     = %3.2e\n',prim_infeas);
            fprintf(' rel. dual   infeas     = %3.2e\n',dual_infeas);
        end
        fprintf(' norm(X), norm(y), norm(Z) = %3.1e, %3.1e, %3.1e\n',...
            info.normX,info.normy,info.normZ);
        fprintf(' norm(A), norm(b), norm(C) = %3.1e, %3.1e, %3.1e\n',...
            info.normA,info.normb,info.normC);
    end
elseif (termcode == 1)
    if (printlevel >=2)
        fprintf(' residual of primal infeasibility      \n')
        fprintf(' certificate (y,Z)      = %3.2e\n',resid);
        fprintf(' reldist to infeas.    <= %3.2e\n',reldist);
    end
elseif (termcode == 2)
    if (printlevel >=2)
        fprintf(' residual of dual infeasibility        \n')
        fprintf(' certificate X          = %3.2e\n',resid);
        fprintf(' reldist to infeas.    <= %3.2e\n',reldist);
    end
end
if (printlevel >=2)
    fprintf(' Total CPU time (secs)  = %3.2f  \n',totaltime);
    fprintf(' CPU time per iteration = %3.2f  \n',totaltime/iter);
    fprintf(' termination code       = %2.0f\n',termcode);
    fprintf(' DIMACS: %.1e  %.1e  %.1e  %.1e  %.1e  %.1e\n',dimacs);
    fprintf('------------------------------------------------');
    fprintf('-------------------\n');
    if (printlevel > 3)
        fprintf(' Percentage of CPU time spent in various parts \n');
        fprintf('------------------------------------------------');
        fprintf('-------------------\n');
        fprintf(' preproc Xchol Zchol pred pred_steplen  corr corr_steplen  misc\n')
        tt = [preproctime, pcholtime, dcholtime, predtime, pstep_predtime, dstep_predtime];
        tt = [tt, corrtime, pstep_corrtime, dstep_corrtime, misctime];
        tt = tt/sum(tt)*100;
        fprintf('   %3.1f   %3.1f   %3.1f   %3.1f  %3.1f  %3.1f ',...
            tt(1),tt(2),tt(3),tt(4),tt(5),tt(6));
        fprintf('    %3.1f  %3.1f  %3.1f     %3.1f\n',tt(7),tt(8),tt(9),tt(10));
        fprintf('------------------------------------------------');
        fprintf('-------------------\n');
    end
end
%%*****************************************************************************


function [SACdata,SeisData,sacfiles]=sacpc2mat(varargin)
% [SACdata,SeisData,filenames] = SACPCMAT('file1','file2',..., 'filen' )
%
% reads n SAC files file1, file2, filen (SAC files are assumed to have
% PC byte order) and converts them to matlab
% format. The filenames can contain globbing characters (e.g. * and ?).
% These are expanded and all matching files loaded.
%
% SACPCMAT( cellarray ) where cellarray={'file1','file2',...,'filen'}
% is equivalent to the standard form.
%
% SACdata is an n x 1 struct array containing the header variables
%         in the same format as is obtained by using MAT function
%         of SAC2000.
%         SACdata(i).trcLen contains the number of samples.
%
% SeisData is an m x n array (where m=max(npts1, npts2, ...) )
%         containing the actual data.
%
% filenames is a n x 1 string cell array with the filenames actually read.
%
% Note that writing
%
%  [SACdata,SeisData] = sacsun2mat('file1','file2',..., 'filen' )
%
% is equivalent to the following sequence
%
% sac2000
% READ file1 file2 .. filen
% MAT
%
% (in fact the failure of above sequence to work properly on my
% system motivated this script).
%
%
% SACPC2MAT was written by F Tilmann (tilmann@esc.cam.ac.uk)
% based on sac_sun2pc_mat  by C. D. Saragiotis (I copied the
% routines doing the actual work from this code but
% used a different header structure and made the routine
% flexible).
% It was tested on MATLAB5 on a PC but
% should work on newer versions, too.
%
% (C) 2004
%

F = 4-1; % float byte-size minus 1;
K = 8-1; % alphanumeric byte-size minus 1
L = 4-1; % long integer byte-size minus 1;

fnames={};
for i=1:nargin
  if ischar(varargin{i})
    fnames=cell([fnames; cellstr(varargin{i})]);
  elseif iscellstr(varargin{i}) & size(varargin{i},1)==1
    fnames=cell([fnames; varargin{i}']);
  elseif iscellstr(varargin{i}) & size(varargin{i},2)==1
    fnames=cell([fnames; varargin{i}]);
  end
end
% expand globs
sacfiles={};k=1;
for i=1:length(fnames)
  dirlist=dir(fnames{i});
  for j=1:length(dirlist)
    if ~dirlist(j).isdir
      sacfiles{k,1}=dirlist(j).name;
      k=k+1;
    end
  end
end

maxnpts=0;
for i=1:length(sacfiles)
  fid=fopen(sacfiles{i},'rb');
  if fid==-1
    error(sprintf('Could not open SAC file %s',fnames{i}))
  end
  SACdata(i,1)=readSacHeader(fid,F,K,L);
  npts=SACdata(i).trcLen;
  if npts>maxnpts
    maxnpts=npts;
  end
  fprintf('Processing file %d: %s\n',i,sacfiles{i});
  SeisData(npts,i)=0;   % Magnify seis matrix if necessary
  SeisData(:,i)=[ readSacData(fid,npts,F+1); zeros(maxnpts-npts,1)];    % Have to pad with zeros if new data have less data points than some previously read file
end

function hdr = readSacHeader(fileId,F,K,L)
% hdr = readSacHeader(FID)
% sacReadAlphaNum reads the SAC-format header fields and returns most of them.
%
%    The output variable, 'hdr' is a structure, whose fields are
%    the fields as in the SACdata structure generated by SAC's
%    matlab command MAT
%
%    Created by C. D. Saragiotis, August 5th, 2003, modified by F Tilmann
headerBytes = 632;
chrctr = fread(fileId,headerBytes,'uchar');
chrctr = chrctr(:)';
% Read floats
hdr.times.delta  = sacReadFloat(chrctr(1:1+F)); % increment between evenly spaced samples
%hdr.DEPMIN = sacReadFloat(chrctr(5:5+F)); % MINimum value of DEPendent variable
%hdr.DEPMAX = sacReadFloat(chrctr(9:9+F)); % MAXimum value of DEPendent variable
% (not currently used) SCALE  = sacReadFloat(chrctr(13:13+F)); % Mult SCALE factor for dependent variable
%hdr.ODELTA = sacReadFloat(chrctr(17:17+F)); % Observd increment if different than DELTA
hdr.times.b      = sacReadFloat(chrctr(21:21+F)); % Begining value of the independent variable
hdr.times.e      = sacReadFloat(chrctr(25:25+F)); % Ending value of the independent variable
hdr.times.o      = sacReadFloat(chrctr(29:29+F)); % event Origin time
hdr.times.a      = sacReadFloat(chrctr(33:33+F)); % first Arrival time
hdr.times.t0 = sacReadFloat(chrctr(41:41+F));
hdr.times.t1 = sacReadFloat(chrctr(45:45+F));
hdr.times.t2 = sacReadFloat(chrctr(49:49+F));
hdr.times.t3 = sacReadFloat(chrctr(53:53+F));
hdr.times.t4 = sacReadFloat(chrctr(57:57+F));
hdr.times.t5 = sacReadFloat(chrctr(61:61+F));
hdr.times.t6 = sacReadFloat(chrctr(65:65+F));
hdr.times.t7 = sacReadFloat(chrctr(69:69+F));
hdr.times.t8 = sacReadFloat(chrctr(73:73+F));
hdr.times.t9 = sacReadFloat(chrctr(77:77+F));
hdr.times.f      = sacReadFloat(chrctr(81:81+F)); % Fini of event time

hdr.response = sacReadFloat(reshape(chrctr(85:85+10*(F+1)-1),F+1,10)');

hdr.station.stla   = sacReadFloat(chrctr(125:125+F)); % STation LAttitude
hdr.station.stlo   = sacReadFloat(chrctr(129:129+F)); % STation LOngtitude
hdr.station.stel   = sacReadFloat(chrctr(133:133+F)); % STation ELevation
hdr.station.stdp   = sacReadFloat(chrctr(137:137+F)); % STation DePth below surface

hdr.event.ecla   = sacReadFloat(chrctr(141:141+F)); % EVent LAttitude
hdr.event.evlo   = sacReadFloat(chrctr(145:145+F)); % EVent LOngtitude
hdr.event.evel   = sacReadFloat(chrctr(149:149+F)); % EVent ELevation
hdr.event.evdp   = sacReadFloat(chrctr(153:153+F)); % EVent DePth below surface
hdr.event.mag   = sacReadFloat(chrctr(157:157+F)); % EVent DePth below surface

userdata=sacReadFloat(reshape(chrctr(161:161+10*(F+1)-1),F+1,10)');

hdr.evsta.dist   = sacReadFloat(chrctr(201:201+F)); % station to event DISTance (km)
hdr.evsta.az     = sacReadFloat(chrctr(205:205+F)); % event to station AZimuth (degrees)
hdr.evsta.baz    = sacReadFloat(chrctr(209:209+F)); % station to event AZimuth (degrees)
hdr.evsta.gcarc  = sacReadFloat(chrctr(213:213+F)); % station to event Great Circle ARC length (degrees)

%hdr.DEPMEN = sacReadFloat(chrctr(225:225+F)); % MEaN value of DEPendent variable

hdr.station.cmpaz  = sacReadFloat(chrctr(229:229+F)); % CoMPonent AZimuth (degrees clockwise from north)
hdr.station.cmpinc = sacReadFloat(chrctr(233:233+F)); % CoMPonent INCident angle (degrees from vertical)

hdr.llnl.xminimum = sacReadFloat(chrctr(237:237+L));
hdr.llnl.xmaximum = sacReadFloat(chrctr(241:241+L));
hdr.llnl.yminimum = sacReadFloat(chrctr(245:245+L));
hdr.llnl.ymaximum = sacReadFloat(chrctr(249:249+L));

% Read long integers
hdr.event.nzyear = sacReadLong(chrctr(281:281+L)); % GMT YEAR
hdr.event.nzjday = sacReadLong(chrctr(285:285+L)); % GMT julian DAY
hdr.event.nzhour = sacReadLong(chrctr(289:289+L)); % GMT HOUR
hdr.event.nzmin  = sacReadLong(chrctr(293:293+L)); % GMT MINute
hdr.event.nzsec  = sacReadLong(chrctr(297:297+L)); % GMT SECond
hdr.event.nzmsec = sacReadLong(chrctr(301:301+L)); % GMT MilliSECond

hdr.llnl.norid = sacReadLong(chrctr(309:309+L));
hdr.llnl.nevid = sacReadLong(chrctr(313:313+L));

hdr.trcLen   = sacReadLong(chrctr(317:317+L)); % Number of PoinTS per data component

hdr.llnl.nwfid = sacReadLong(chrctr(325:325+L));
hdr.llnl.nxsize = sacReadLong(chrctr(329:329+L));
hdr.llnl.nysize = sacReadLong(chrctr(333:333+L));

hdr.descrip.iftype = sacReadLong(chrctr(341:341+L)); % File TYPE
hdr.descrip.idep   = sacReadLong(chrctr(345:345+L)); % type of DEPendent variable
hdr.descrip.iztype  = sacReadLong(chrctr(349:349+L)); % reference time equivalence

hdr.descrip.iinst  = sacReadLong(chrctr(357:357+L)); % type of recording INSTrument

% Before there were floats read here?
hdr.descrip.istreg = sacReadLong(chrctr(361:361+F)); % STation geographic REGion
hdr.descrip.ievreg = sacReadLong(chrctr(365:365+F)); % EVent geographic REGion
hdr.descrip.ievtyp = sacReadLong(chrctr(369:369+F)); % EVent geographic REGion
hdr.descrip.iqual  = sacReadLong(chrctr(373:373+F)); % QUALity of data
hdr.descrip.isynth = sacReadLong(chrctr(377:377+F)); % SYNTHetic data flag

hdr.event.imagtyp  = sacReadLong(chrctr(381:381+F));
hdr.event.imagsrc  = sacReadLong(chrctr(385:385+F));

% no logical SAC header variables in matlab SACdata structure!
% previous version set these defaults
% $$$ % SAC defaults
% $$$ hdr.IEVTYPE = 'IUNKN';
% $$$ %IEVTYPE= sacReadFloat(chrctr(369:369+F)); % EVent TYPE
% $$$ hdr.LEVEN = 'TRUE';  % true, if data are EVENly spaced (required)
% $$$ %LEVEN= sacReadFloat(chrctr(421:521+F)); % true, if data are EVENly spaced (required)
% $$$ hdr.LPSPOL = 'FALSE'; % true, if station components have a PoSitive POLarity
% $$$ %LPSPOL= sacReadFloat(chrctr(425:425+F)); % true, if station components have a PoSitive POLarity
% $$$ hdr.LOVROK = 'FALSE'; % true, if it is OK to OVeRwrite this file in disk
% $$$ %LOVROK= sacReadFloat(chrctr(429:429+F)); % true, if it is OK to OVeRwrite this file in disk
% $$$ hdr.LCALDA = 'TRUE'; % true, if DIST, AZ, BAZ and GCARC are to be calculated from station and event coordinates
% $$$ %LCALDA= sacReadFloat(chrctr(433:433+F)); % true, if DIST, AZ, BAZ and GCARC are to be calculated from station and event coordinates

% Read alphanumeric data
hdr.station.kstnm  = sacReadAlphaNum(chrctr(441:441+K)); % STation NaMe
hdr.event.kevnm  = sacReadAlphaNum(chrctr(449:449+2*(K+1)-1)); % EVent NaMe
%hdr.KHOLE  = sacReadAlphaNum(chrctr(465:465+K)); % HOLE identification, if nuclear event
hdr.times.ko     = sacReadAlphaNum(chrctr(473:473+K)); % event Origin time identification
hdr.times.ka     = sacReadAlphaNum(chrctr(481:481+K)); % first Arrival time identification

hdr.times.kt0 = sacReadAlphaNum(chrctr(489:489+K));
hdr.times.kt1 = sacReadAlphaNum(chrctr(497:497+K));
hdr.times.kt2 = sacReadAlphaNum(chrctr(505:505+K));
hdr.times.kt3 = sacReadAlphaNum(chrctr(513:513+K));
hdr.times.kt4 = sacReadAlphaNum(chrctr(521:521+K));
hdr.times.kt5 = sacReadAlphaNum(chrctr(529:529+K));
hdr.times.kt6 = sacReadAlphaNum(chrctr(537:537+K));
hdr.times.kt7 = sacReadAlphaNum(chrctr(545:545+K));
hdr.times.kt8 = sacReadAlphaNum(chrctr(553:553+K));
hdr.times.kt9 = sacReadAlphaNum(chrctr(561:561+K));

hdr.times.kf     = sacReadAlphaNum(chrctr(569:569+K)); % Fini identification
kuser0 = sacReadAlphaNum(chrctr(577:577+K)); % USER-defined variable storage area
kuser1 = sacReadAlphaNum(chrctr(585:585+K)); % USER-defined variable storage area
kuser2 = sacReadAlphaNum(chrctr(593:593+K)); % USER-defined variable storage area
hdr.station.kcmpnm = sacReadAlphaNum(chrctr(601:601+K)); % CoMPonent NaMe
hdr.station.knetwk = sacReadAlphaNum(chrctr(609:609+K)); % name of seismic NETWorK
%hdr.KDATRD = sacReadAlphaNum(chrctr(617:617+K)); % DATa Recording Date onto the computer
%hdr.KINST  = sacReadAlphaNum(chrctr(625:625+K)); % generic name of recording INSTrument

usercell=num2cell(userdata);
[usercell{find(userdata==-12345)}]=deal([]);
[hdr.user(1:10).data]=deal(usercell{:});
[hdr.user(1:10).label]=deal(kuser0, kuser1,kuser2,[], [], [], [], [], [], []);

function X = readSacData(fid,N,F)
% function data = readSacData('filename',NPTS,floatByteSize)
chrctr = fread(fid,N*F,'uchar');
x=reshape(chrctr,F,N)';
%x
X = sacReadFloat(x);

function lNumber = sacReadLong(cb)
% reads long integers (4 bytes long)
% cb is the character buffer
cb = cb(:);
% SUN sac format use: lNumber = (256.^(3:-1:0))*cb;
lNumber = (256.^(0:3))*cb;        % changed line
if lNumber == -12345, lNumber = []; end

function fNumber = sacReadFloat(cb)
% reads floats (4 bytes long)
% cb is the character buffer
C = size(cb,1);
% SUN sac format use: stringOfBitSequence = [dec2bin(cb(:,1),8) dec2bin(cb(:,2),8) dec2bin(cb(:,3),8) dec2bin(cb(:,4),8)];
stringOfBitSequence = [dec2bin(cb(:,4),8) dec2bin(cb(:,3),8) ...
            dec2bin(cb(:,2),8) dec2bin(cb(:,1),8)];  %changed line
bitSequence = stringOfBitSequence=='1';
fSign = -2*bitSequence(:,1)+1;
fExp = bitSequence(:,2:9)*(2.^(7:-1:0)') - 127;
fMantissa = [ones(C,1) bitSequence(:,10:32)]*(2.^(0:-1:-23)');
fNumber = fSign.*fMantissa.*(2.^fExp);
isZeroCheck = sum(bitSequence')'==0;
fNumber = fNumber.*(~isZeroCheck);
if C==1 & fNumber == -12345, fNumber = []; end

function alphaNum = sacReadAlphaNum(cb)
% reads alphanumeric data (8 or 16 bytes long). If it cb is empty, it returns a ' '
% cb is the character buffer
K = max(size(cb));
alphaNumTemp = char(cb);
if K == 8
    if alphaNumTemp == '-12345  '
        alphaNum = [];
    else
        alphaNum = alphaNumTemp;
    end
else
    if K == 16
        if alphaNumTemp == '-12345   -12345 ' | alphaNumTemp == '-12345          '
            alphaNum = [];
        else
            alphaNum = alphaNumTemp;
        end
    end
end