%Camera  Camera superclass
%
% An abstract superclass for Toolbox camera classes.
%
% Methods::
%
% plot           plot projection of world point to image plane
% hold           control figure hold for image plane window
% ishold         test figure hold for image plane
% clf            clear image plane
% figure         figure holding the image plane
% mesh           draw shape represented as a mesh
% point          draw homogeneous points on image plane
% homline        draw homogeneous lines on image plane
% lineseg        draw line segment defined by points
% plot_camera    draw camera in world view
%-
% rpy            set camera attitude
% move           clone Camera after motion
% centre         get world coordinate of camera centre
%-
% delete         object destructor
% char           convert camera parameters to string
% display        display camera parameters
%-
% Properties (read/write)::
% npix    image dimensions (2x1)
% pp      principal point (2x1)
% rho     pixel dimensions (2x1) in metres
% T       camera pose as homogeneous transformation
%
% Properties (read only)::
% nu    number of pixels in u-direction
% nv    number of pixels in v-direction
% u0    principal point u-coordinate
% v0    principal point v-coordinate
%
% Notes::
%  - Camera is a reference object.
%  - Camera objects can be used in vectors and arrays
%  - This is an abstract class and must be subclassed and a project()
%    method defined.
%  - The object can create a window to display the Camera image plane, this
%    window is protected and can only be accessed by the plot methods of
%    this object.
%  - The project method is implemented by the concrete subclass.
%
% See also CentralCamera, SphericalCamera, FishEyeCamera, CatadiptricCamera.

% Copyright (C) 1995-2009, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB 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.
%
% MVTB 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 MVTB.  If not, see <http://www.gnu.org/licenses/>.

% TODO:
%   make a parent imaging class and subclass perspective, fisheye, panocam
%   test for points in front of camera and set to NaN if not
%   test for points off the image plane and set to NaN if not
%     make clipping/test flags
classdef Camera < handle

    properties
        name    % camera name
        type
        rho     % pixel dimensions 1x2
        pp      % principal point 1x2
        npix    % number of pixel 1x2
        T       % camera pose SE3 object
        noise   % pixel noise 1x2
        image
    end

    properties (SetAccess = protected)
        limits
        perspective
        h_imageplane     % handle for image plane
        h_3dview % handle for camera 3D view
        P           % world points (last plotted)
        holdon
        color
    end

    properties (Dependent = true, SetAccess = protected)
        u0
        v0
        nu
        nv
    end

    methods (Abstract)
        p = project(c, P, varargin);
    end

    methods

        function c = Camera(varargin)
            %Camera.Camera Create camera object
            %
            % Constructor for abstact Camera class, used by all subclasses.
            %
            % C = Camera(OPTIONS) creates a default (abstract) camera with null parameters.
            %
            % Options::
            % 'name',N          Name of camera
            % 'image',IM        Load image IM to image plane
            % 'resolution',N    Image plane resolution: NxN or N=[W H]
            % 'sensor',S        Image sensor size in metres (2x1) [metres]
            % 'centre',P        Principal point (2x1)
            % 'pixel',S         Pixel size: SxS or S=[W H]
            % 'noise',SIGMA     Standard deviation of additive Gaussian noise added to
            %                   returned image projections
            % 'pose',T          Pose of the camera as a homogeneous transformation
            % 'color',C         Color of image plane background (default [1 1 0.8])
            %
            % Notes::
            % - Normally the class plots points and lines into a set of axes that represent
            %   the image plane.  The 'image' option paints the specified image onto the
            %   image plane and allows points and lines to be overlaid.
            %
            % See also CentralCamera, FisheyeCamera, CatadioptricCamera, SphericalCamera.

            % default values
            c.type = '**abstract**';
            c.T = eye(4,4);
            c.pp = [];
            c.limits = [-1 1 -1 1];
            c.perspective = false;
            c.h_imageplane = [];
            c.h_3dview = [];
            c.holdon = false;
            c.color = [1 1 0.8];

            if nargin == 1 && isa(varargin{1}, 'Camera')
                return;
            else
                opt.name = 'noname';
                opt.image = [];
                opt.resolution = [];
                opt.centre = [];
                opt.sensor = [];
                opt.pixel = [1 1];
                opt.noise = [];
                opt.pose = [];
                opt.color = [];
                c.pp = [0 0];

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

                c.name = opt.name;
                if ~isempty(opt.image)
                    c.image = opt.image;
                    c.npix = [size(c.image,2) size(c.image,1)];
                end
                if ~isempty(opt.resolution)
                    if length(opt.resolution) == 1
                        c.npix = [opt.resolution opt.resolution];
                    elseif length(opt.resolution) == 2
                        c.npix = opt.resolution;
                    else
                        error('resolution must be a 1- or 2-vector');
                    end
                end

                c.pp = opt.centre;
                c.rho = opt.pixel;
                if ~isempty(opt.color)
                    c.color = opt.color;
                end
                if ~isempty(opt.noise)
                    if length(opt.noise) == 1
                        c.noise = [opt.noise opt.noise];
                    elseif length(opt.noise) == 2
                        c.noise = opt.noise;
                    else
                        error('noise must be a 1- or 2-vector');
                    end
                end
                if ~isempty(opt.pose)
                    c.T = SE3(opt.pose);
                end
                if ~isempty(opt.sensor)
                    c.rho = opt.sensor ./ c.npix;
                end
            end

            if length(c.rho) == 1
                c.rho = ones(1,2) * c.rho;
            end
            if isempty(c.pp)
                fprintf('principal point not specified, setting it to centre of image plane\n');
                c.pp = c.npix / 2;
            end
        end

        function delete(c)
            %Camera.delete Camera object destructor
            %
            % C.delete() destroys all figures associated with the Camera object and
            % removes the object.
            %disp('delete camera object');
            if ~isempty(c.h_imageplane) && isgraphics(c.h_imageplane)
                delete(get(c.h_imageplane, 'Parent'));
            end
            if ~isempty(c.h_3dview) && isgraphics(c.h_3dview)
                delete(get(c.h_3dview, 'Parent'));
            end
        end

        function display(c)
            %Camera.display Display value
            %
            % C.display() displays a compact human-readable representation of the camera
            % parameters.
            %
            % Notes::
            % - This method is invoked implicitly at the command line when the result
            %   of an expression is a Camera object and the command has no trailing
            %   semicolon.
            %
            % See also Camera.char.
            loose = strcmp( get(0, 'FormatSpacing'), 'loose');
            if loose
                disp(' ');
            end
            disp([inputname(1), ' = '])
            if loose
                disp(' ');
            end
            disp(char(c))
            if loose
                disp(' ');
            end
        end

        function s = char(c, s)
            %Camera.char Convert to string
            %
            % S = C.char() is a compact string representation of the camera parameters.

            s = '';
            if ~isempty(c.rho)
                s = strvcat(s, sprintf('  pixel size:     (%.4g, %.4g)', c.rho(1), c.rho(2)));
            end
            if ~isempty(c.pp)
                s = strvcat(s, sprintf('  principal pt:   (%.4g, %.4g)', c.u0, c.v0));
            end
            if ~isempty(c.npix)
                s = strvcat(s, sprintf('  number pixels:  %d x %d', c.nu, c.nv));
            end
            if ~isempty(c.noise)
                s = strvcat(s, sprintf('  noise:          %.4g,%.4g pix', c.noise));
            end
            s = strvcat(s,     sprintf('  pose:           %s', print(c.T, 'camera')));
            % = strvcat(s, [repmat('      ', 4,1) num2str(c.T)]);
        end

        function rpy(c, roll, pitch, yaw)
            %Camera.rpy Set camera attitude
            %
            % C.rpy(R, P, Y) sets the camera attitude to the specified roll-pitch-yaw angles.
            %
            % C.rpy(RPY) as above but RPY=[R,P,Y].
            if nargin == 2,
                pitch = roll(2);
                yaw = roll(3);
                roll = roll(1);
            end
            c.T = SE3.Ry(yaw) * SE3.Rx(pitch) * SE3.Rz(roll);
        end

        function c = centre(c)
            %Camera.centre Get camera position
            %
            % P = C.centre() is the 3-dimensional position of the camera centre (3x1).

            c = c.T.t;
        end

        function ishold = hold(c, flag)
            %Camera.hold Control hold on image plane graphics
            %
            % C.hold() sets "hold on" for the camera's image plane.
            %
            % C.hold(H) hold mode is set on if H is true (or > 0), and off if
            % H is false (or 0).

            % cam.ishold(); set hold
            % cam.ishold(flag); set hold
            if nargin < 2
                flag = true;
            end
            c.holdon = flag;
            if flag
                set(c.h_imageplane, 'NextPlot', 'add');
            else
                set(c.h_imageplane, 'NextPlot', 'replacechildren');
            end
            if nargout > 0
                % i = cam.ishold(); test hold condition
                ishold = c.holdon;

            end
        end

        function v = ishold(c)
            %Camera.ishold Return image plane hold status
            %
            % H = C.ishold() returns true (1) if the camera's image plane is in hold mode,
            % otherwise false (0).
            v = c.holdon();
        end

        function clf(c, flag)
            %Camera.clf Clear the image plane
            %
            % C.clf() removes all graphics from the camera's image plane.
            h = c.h_imageplane;
            if ~isempty(h) && isgraphics(h)
                % remove all children of the figure
                children = get(h, 'Children');
                for child=children
                    delete(child)
                end

                % if the camera is displaying an image
                if ~isempty(c.image)
                    c.figure
                    idisp(c.image, 'nogui');
                    hold on
                end
            end
        end

        function h = figure(c)
            %Camera.figure Return figure handle
            %
            % H = C.figure() is the handle of the figure that contains the camera's
            % image plane graphics.
            fig  = get(c.h_imageplane, 'Parent');
            figure( fig );
            if nargout > 0
                h = fig;
            end
        end

        % Return the graphics handle for this camera's image plane
        % and create the graphics if it doesnt exist
        %
        function h = plot_create(c, hin)

            if ~isempty(c.image)
                % if this camera is created from an image, then display that image

                if isempty(c.h_imageplane) || ~isgraphics(c.h_imageplane)
                    idisp(c.image, 'nogui');
                    set(gcf, 'name', sprintf('%s(%s) - image plane', class(c), c.name));
                    set(gcf, 'MenuBar', 'none');
                    hold on
                    h = gca;
                    %title(h, ['CentralCamera image plane:' c.name);
                    c.h_imageplane = h;
                    set(gcf, 'HandleVisibility', 'off');
                    set(h, 'HandleVisibility', 'off');
                else
                    h = c.h_imageplane;
                end
                return;
            end

            if isgraphics(c.h_imageplane)
                % it already exists, just return the handle
                h = c.h_imageplane;
                return;
            end
            c.h_imageplane = [];  % handle is invalid

            if (nargin == 2) && isgraphics(hin)
                % draw camera in an existing axes
                disp('draw image plane in existing axes')
                h = hin;
                h.HandleVisibility = 'Off';
            else
                disp('creating new figure for camera')

                figure
                h = axes
                fig = get(h, 'Parent');
                disp('make axes');
                axis square
                set(fig, 'MenuBar', 'none');
                set(fig, 'Tag', 'camera');
                set(h, 'Color', c.color);
                set(fig, 'HandleVisibility', 'off');
                set(fig, 'name', sprintf('%s(%s) - image plane', class(c), c.name));
            end
            % create an axis for camera view
            set(h, 'XLim', c.limits(1:2), 'YLim', c.limits(3:4), ...
                'DataAspectRatio', [1 1 1], ...
                'Xgrid', 'on', 'Ygrid', 'on', ...
                'Ydir' , 'reverse', ...
                'NextPlot', 'add', ...
                'Tag', c.name ...
                );
            c.h_imageplane = h;       % keep this around
            c.labelaxes();

            title(h, c.name);
            %figure( fig );   % raise the camera view
            set(h, 'NextPlot', 'replacechildren');
        end

        function labelaxes(c)
            h = c.h_imageplane;

            if ~isempty(c.npix)
                xlabel(h, 'u (pixels)');
                ylabel(h, 'v (pixels)');
            else
                xlabel(h, 'x (m)');
                ylabel(h, 'y (m)');
            end
        end

        function v =  plot(c, points, varargin)
            %Camera.plot Plot points on image plane
            %
            % C.plot(P, OPTIONS) projects world points P (3xN) to the image plane and plots them.  If P is 2xN
            % the points are assumed to be image plane coordinates and are plotted directly.
            %
            % UV = C.plot(P) as above but returns the image plane coordinates UV (2xN).
            %
            % - If P has 3 dimensions (3xNxS) then it is considered a sequence of point sets and is
            %   displayed as an animation.
            %
            % C.plot(L, OPTIONS) projects the world lines represented by the
            % array of Plucker objects (1xN) to the image plane and plots them.
            %
            % LI = C.plot(L, OPTIONS) as above but returns an array (3xN) of
            % image plane lines in homogeneous form.
            %
            % Options::
            % 'objpose',T     Transform all points by the homogeneous transformation T before
            %                  projecting them to the camera image plane.
            % 'pose',T         Set the camera pose to the homogeneous transformation T before
            %                  projecting points to the camera image plane.  Overrides the current
            %                  camera pose C.T.
            % 'fps',N          Number of frames per second for point sequence display
            % 'sequence'       Annotate the points with their index
            % 'textcolor',C    Text color for annotation (default black)
            % 'textsize',S     Text size for annotation (default 12)
            % 'drawnow'        Execute MATLAB drawnow function
            %
            % Additional options are considered MATLAB line or marker style parameters and are passed
            % directly to plot.
            %
            % See also Camera.mesh, Camera.hold, Camera.clf, Plucker.

            opt.fps = 5;
            opt.sequence = false;
            opt.textcolor = 'k';
            opt.textsize = 12;
            opt.drawnow = false;

            [opt,arglist,ls] = tb_optparse(opt, varargin);

            % get handle for this camera image plane
            h = c.plot_create();

            if isa(points, 'Plucker')
                % plot lines

                % project 3D world lines using the class project() method
                uv = c.project(points, arglist{:});
                c.hold(true);
                for line=uv
                    c.homline(line);
                end
                c.hold(false)
            else
                % plot points
                nr = numrows(points);

                if nr == 3
                    % project 3D world points using the class project() method
                    uv = c.project(points, arglist{:});
                else
                    uv = points;
                end

                if isempty(ls) || ~any(cellfun(@(x) ischar(x)&&contains(x, 'Marker'), arglist))
                    % set default style if none given
                    ls = {'Marker', 'o', 'MarkerFaceColor', 'k', 'MarkerEdgeColor', 'k', 'LineStyle', 'none'};
                else
                    ls = {ls};
                end

                for i=1:size(uv,3)
                    % for every frame in the animation sequence
                    plot(uv(1,:,i), uv(2,:,i), ls{:}, 'Parent', h);
                    if opt.sequence
                        for j=1:size(uv,2)
                            text(uv(1,j,i), uv(2,j,i), sprintf('  %d', j), ...
                                'HorizontalAlignment', 'left', ...
                                'VerticalAlignment', 'middle', ...
                                'FontUnits', 'pixels', ...
                                'FontSize', opt.textsize, ...
                                'Color', opt.textcolor, ...
                                'Parent', h);
                        end
                    end

                    if size(uv,3) > 1
                        pause(1/opt.fps);
                    end
                end
            end

            if opt.drawnow
                drawnow
            end

            if nargout > 0,
                v = uv;
            end
        end % plot

        function mesh(c, X, Y, Z, varargin)
            %Camera.mesh Plot mesh object on image plane
            %
            % C.mesh(X, Y, Z, OPTIONS) projects a 3D shape defined by the matrices X, Y, Z
            % to the image plane and plots them.  The matrices X, Y, Z are of the same size
            % and the corresponding elements of the matrices define 3D points.
            %
            % Options::
            % 'objpose',T   Transform all points by the homogeneous transformation T before
            %               projecting them to the camera image plane.
            % 'pose',T      Set the camera pose to the homogeneous transformation T before
            %               projecting points to the camera image plane.  Temporarily overrides
            %               the current camera pose C.T.
            %
            % Additional arguments are passed to plot as line style parameters.
            %
            % See also MESH, CYLINDER, SPHERE, MKCUBE, Camera.plot, Camera.hold, Camera.clf.

            % check that mesh matrices conform
            assert( all(size(X) == size(Y)) && all(size(X) == size(Z)), 'matrices must be the same size');

            opt.objpose = [];
            opt.pose = [];

            [opt,arglist,ls] = tb_optparse(opt, varargin);
            if isempty(opt.pose)
                opt.pose = c.T;
            end

            if isempty(ls)
                ls = {'k'};
            end

            % get handle for this camera image plane
            h = c.plot_create();

            % draw 3D line segments
            nsteps = 21;

            c.clf
            holdon = c.hold(1);
            s = linspace(0, 1, nsteps);

            for i=1:numrows(X)-1
                for j=1:numcols(X)-1
                    P0 = [X(i,j), Y(i,j), Z(i,j)]';
                    P1 = [X(i+1,j), Y(i+1,j), Z(i+1,j)]';
                    P2 = [X(i,j+1), Y(i,j+1), Z(i,j+1)]';

                    if c.perspective
                        % straight world lines are straight on the image plane
                        uv = c.project([P0 P1], 'setopt', opt);
                    else
                        % straight world lines are not straight, plot them piecewise
                        P = bsxfun(@times, (1-s), P0) + bsxfun(@times, s, P1);
                        uv = c.project(P, 'setopt', opt);
                    end
                    plot(uv(1,:)', uv(2,:)', ls{:}, arglist{:}, 'Parent', c.h_imageplane);

                    if c.perspective
                        % straight world lines are straight on the image plane
                        uv = c.project([P0 P2], 'setopt', opt);
                    else
                        % straight world lines are not straight, plot them piecewise
                        P = bsxfun(@times, (1-s), P0) + bsxfun(@times, s, P2);
                        uv = c.project(P, 'setopt', opt);
                    end
                    plot(uv(1,:)', uv(2,:)', ls{:}, arglist{:}, 'Parent', c.h_imageplane);
                end
            end

            for j=1:numcols(X)-1
                P0 = [X(end,j), Y(end,j), Z(end,j)]';
                P1 = [X(end,j+1), Y(end,j+1), Z(end,j+1)]';

                if c.perspective
                    % straight world lines are straight on the image plane
                    uv = c.project([P0 P1], 'setopt', opt);
                else
                    % straight world lines are not straight, plot them piecewise
                    P = bsxfun(@times, (1-s), P0) + bsxfun(@times, s, P1);
                    uv = c.project(P, 'setopt', opt);
                end
                plot(uv(1,:)', uv(2,:)', ls{:}, arglist{:}, 'Parent', c.h_imageplane);
            end
            c.hold(holdon); % turn hold off if it was initially off

        end % mesh

        function h =  point(c, p, varargin)
            %Camera.point Plot homogeneous points on image plane
            %
            % C.point(P) plots points on the camera image plane which are defined by columns
            % of P (3xN) considered as points in homogeneous form.

            % get handle for this camera image plane
            h = c.create

            uv = e2h(p);
            h = plot(uv(1,:), uv(2,:), varargin{:});
        end % point

        function h =  homline(c, lines, varargin)
            %Camera.homline Plot homogeneous lines on image plane
            %
            % C.homline(L) plots lines on the camera image plane which are defined by columns
            % of L (3xN) considered as lines in homogeneous form: a.u + b.v + c = 0.

            % get handle for this camera image plane
            h = c.plot_create;
            xlim = get(h, 'XLim');
            ylim = get(h, 'YLim');

            if numel(lines) == 3
                lines = lines(:);
            end

            for l=lines
                if abs(l(1)/l(2)) > 1
                    % steeper than 45deg
                    x = (-l(3) - l(2)*ylim) / l(1);
                    h = plot(x, ylim, varargin{:}, 'Parent', c.h_imageplane);
                else
                    % less than 45deg
                    y = (-l(3) - l(1)*xlim) / l(2);

                    h = plot(xlim, y, varargin{:}, 'Parent', c.h_imageplane);
                end
            end
        end % line

        function h = lineseg(c, p0, p1, varargin)
            % get handle for this camera image plane
            h = c.plot_create
            c.hold(1)
            for i=1:numcols(p0)
                plot([p0(1,i) p1(1,i)], [p0(2,i) p1(2,i)], varargin{:}, 'Parent', c.h_imageplane);
            end
        end

        function newcam = move(cam, T)
            %Camera.move Instantiate displaced camera
            %
            % C2 = C.move(T) is a new camera object that is a clone of C but its pose
            % is displaced by the homogeneous transformation T with respect to the
            % current pose of C.
            newcam = CentralCamera(cam);
            newcam.T = newcam.T * SE3.convert(T);
        end

        function movedby(c, robot)
            robot.addlistener('Moved', @(src,data)cameramove_callback(src,data,c));

            function cameramove_callback(robot, event, camera)
                camera.T = robot.fkine(robot.q);
            end
        end

        % return components of principal point and image size
        function v = get.u0(c)
            v = c.pp(1);
        end

        function v = get.v0(c)
            v = c.pp(2);
        end

        function v = get.rho(c)
            v = c.rho;
        end

        function v = get.nu(c)
            v = c.npix(1);
        end

        function v = get.nv(c)
            v = c.npix(2);
        end

        function c = set.T(c, Tc)
            c.T = SE3(Tc);

            if ~isempty(c.h_3dview) && isgraphics(c.h_3dview)
                set(c.h_3dview, 'Matrix', c.T.T);
            end
        end

        function c = set.rho(c, sxy)
            if isempty(sxy)
                c.rho = sxy;
            elseif length(sxy) == 1
                c.rho = [sxy sxy];
            elseif length(sxy) == 2
                c.rho = sxy(:)';
            else
                error('need 1 or 2 scale elements');
            end
        end

        function c = set.pp(c, pp)
            if isempty(pp)
                c.pp = [];
            elseif length(pp) == 1
                c.pp = [pp pp];
            elseif length(pp) == 2
                c.pp = pp(:)';
            else
                error('need 1 or 2 pp elements');
            end
        end

        function c = set.npix(c, npix)
            if ~isempty(npix)
                if length(npix) == 1,
                    c.npix = [npix npix];
                elseif length(npix) == 2,
                    c.npix = npix(:)';
                else
                    error('need 1 or 2 npix elements');
                end
                c.limits = [0 c.npix(1) 0 c.npix(2)];
            end
        end

        function help(c)
            disp(' C.plot(P)     return image coordinates for world points  P');
            disp(' C.point(P)     return image coordinates for world points  P');
            disp(' C.line(P)     return image coordinates for world points  P');
            disp(' C.clf     return image coordinates for world points  P');
            disp(' C.hold     return image coordinates for world points  P');
            disp(' C.project(P)     return image coordinates for world points  P');
            disp(' C.project(P, Tobj)  return image coordinates for world points P ');
            disp(' C.project(P, To, Tcm)  return image coordinates for world points P ');
            disp(' transformed by T prior to projection');
        end
    end % methods

end % class


function dummyObject = fixDummyObjectSize(dummyObject,originalObject);

frameUpdateInterval = dummyObject.frameUpdateInterval;
endMargin = size(originalObject.spectrogram,2)*frameUpdateInterval-max(originalObject.anchorTimeLocation);
if size(dummyObject.spectrogram,2)*frameUpdateInterval < max(dummyObject.anchorTimeLocation)+endMargin
    dummyFrameSize = max(dummyObject.anchorTimeLocation)+endMargin;
    dimmyFrequencySize = size(originalObject.spectrogram,1);
    dummyObject.spectrogram = ones(dimmyFrequencySize,dummyFrameSize);
    dummyObject.aperiodicityIndex = ones(dimmyFrequencySize,dummyFrameSize);
    dummyObject.F0 = ones(1,dummyFrameSize);
    dummyObject.vuv = ones(1,dummyFrameSize);
end;


% test find root node
clear;clc;close all;
path(path,'toolbox') ;
t3 = 6; % for small cycles;
% sk_filename='../result/cylinder1_contract_t(2)_nn(14)_WL(10.633697)_WH(1.000000)_sl(3.000000)_skeleton.mat';
% sk_filename='../result/simplejoint_v4770_contract_t(3)_nn(30)_WL(15.378798)_WH(1.000000)_sl(3.000000)_skeleton.mat';
sk_filename='../result/horse_v1987_contract_t(3)_nn(24)_WL(7.786614)_WH(1.000000)_sl(3.000000)_skeleton.mat';

load(sk_filename,'M');

%%
[joints, segments] = find_joints(M,false);
[joints, segments] = remove_small_cycles(M, joints, segments,t3, true);


function kern = gaussianwhiteKernExpandParam(kern, params)

% GAUSSIANWHITEKERNEXPANDPARAM Create kernel structure from gaussian white
%                              kernel's parameters.
% FORMAT
% DESC returns a gaussian white kernel structure filled with the parameters in the given
%	vector. This is used as a helper function to enable parameters to be
%	optimised in, for example, the NETLAB optimisation functions.
% RETURN kern : kernel structure with the given parameters in the relevant
%	   locations.
% ARG kern : the kernel structure in which the parameters are to be
% ARG param : vector of parameters which are to be placed in the kernel
%	   structure.
%
% SEEALSO : gaussianwhiteKernParamInit, gaussianwhiteKernExtractParam, kernExpandParam
%
% COPYRIGHT : Mauricio Alvarez and Neil D. Lawrence, 2008
%
% MODIFICATIONS : Mauricio A. Alvarez, 2009

% KERN

if kern.isArd
    if kern.nIndFunct == 1,
        kern.precisionT =  params(1:end-1)';
    else
        kern.precisionT = reshape(params(1:end-1), size(kern.precisionT));
    end
else
    kern.precisionT =  params(1:end-1);
end
kern.sigma2Noise = params(end);


function [cl,cu] = matRad_getConstraintBounds(optiProb,cst)
% matRad IPOPT get constraint bounds function for direct aperture optimization
%
% call
%   [cl,cu] = matRad_daoGetConstBounds(optiProb,cst)
%
% input
%   optiProb:   option struct defining the type of optimization
%   cst:        matRad cst struct
%
% output
%   cl:         lower bounds on constraints
%   cu:         lower bounds on constraints
%
% References
%
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2015 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

apertureInfo = optiProb.apertureInfo;

% Initialize bounds
cl_dao = zeros(apertureInfo.totalNumOfLeafPairs,1);
cu_dao = inf*ones(apertureInfo.totalNumOfLeafPairs,1);

% get dosimetric bounds from cst (just like for conv opt) by call to
% superclass method
[cl_dos,cu_dos] = matRad_getConstraintBounds@matRad_OptimizationProblem(optiProb,cst);

% concatenate
cl = [cl_dao; cl_dos];
cu = [cu_dao; cu_dos];


function diri = sb_find_elec(vol,sens)

% SB_FIND_ELEC
%
% $Id$

diri = zeros(size(sens.elecpos,1),1);
dist = zeros(size(sens.elecpos,1),1);
for i=1:size(sens.elecpos,1)
    [dist(i), diri(i)] = min(sum(bsxfun(@minus,vol.pos,sens.elecpos(i,:)).^2,2));
end
if ~all(dist < 1e-8)
    error('Electrode positions are not located on mesh nodes! This should not happen, please contact support.');
end
end


function obj = runICPTransformFinal(obj, p, g)

msg('S', {g.proc{:} 'TRANSFORMATION'}, 'LogLevel', 'basic');

for i = 1:g.nPC

    % Find new path
    [~, file] = fileparts(obj.PC{i});
    p2mat = fullfile(obj.OutputFolder, [file '_POSTICP.mat']);

    if ~ismember(i, p.IdxFixedPointClouds) % trafo only if point cloud is not fixed, i.e. loose

        % Load point cloud
        PC = obj.loadPC(i);

        % Trafo
        PC.transform(1, obj.D.H{1,i}(1:3,1:3), obj.D.H{1,i}(1:3,4));

        % Update mat file
        PC.save(p2mat);

    else

        % Copy original mat file
        copyfile(obj.PC{i}, p2mat);

    end

    % Set new path
    obj.PC{i} = p2mat;

end

msg('E', {g.proc{:} 'TRANSFORMATION'}, 'LogLevel', 'basic');

end


% -*- INTERNAL UNDOCUMENTED FUNCTION -*-
function [geometry, msh, spv, vel, gnum, spp, press, gnump] = ...
              mp_solve_stokes_3d (problem_data, method_data)

warning ('geopdes:obsolete','Function MP_SOLVE_STOKES_3D is obsolete. Using MP_SOLVE_STOKES instead')

function [geometry, msh, spv, vel, gnum, spp, press, gnump] = ...
              mp_solve_stokes (problem_data, method_data)


% -*- INTERNAL UNDOCUMENTED FUNCTION -*-
function [geometry, msh, space, eigv, eigf, gnum, dofs_ornt] = ...
              mp_solve_maxwell_eig_2d (problem_data, method_data)

warning ('geopdes:obsolete','Function MP_SOLVE_MAXWELL_EIG_2D is obsolete. Using SOLVE_MAXWELL_EIG instead')

[geometry, msh, space, eigv, eigf, gnum, dofs_ornt] = mp_solve_maxwell_eig (problem_data, method_data);


% ------------------------------------------------------------------------
%      Analyze non-linear ROIs that were clicked as 'object points'
% ------------------------------------------------------------------------

%% ENTER PARAMETERS AND FILE LOCATION

% file location of object points
save_folder = 'C:\Drive\Histology\brainX\processed';

% directory of reference atlas files
annotation_volume_location = 'C:\Drive\Histology\for tutorial\annotation_volume_10um_by_index.npy';
structure_tree_location = 'C:\Drive\Histology\for tutorial\structure_tree_safe_2017.csv';

% name of the saved object points
object_save_name_suffix = '';

% either set to 'all' or a list of indices from the clicked objects in this file, e.g. [2,3]
objects_to_analyze = 'all';

% plane used to view when points were clicked ('coronal' -- most common, 'sagittal', 'transverse')
plane = 'coronal';

% brain figure black or white
black_brain = true;

%% LOAD THE REFERENCE ANNOTATIONS AND PROBE POINTS

% load the reference brain annotations
if ~exist('av','var') || ~exist('st','var')
    disp('loading reference atlas...')
    av = readNPY(annotation_volume_location);
    st = loadStructureTree(structure_tree_location);
end

% load object points
objectPoints = load(fullfile(save_folder, ['probe_points' object_save_name_suffix]));

% determine which objects to analyze
if strcmp(objects_to_analyze,'all')
    objects = 1:size(objectPoints.pointList.pointList,1);
else
    objects = objects_to_analyze;
end

%% BRING UP THE RELEVANT DATA FOR EACH PROBE POINTS, FOR FURTHER ANALYSIS

% initialize cell array containing info on each clicked point
if length(objects) > 1
    roi_annotation = cell(length(objects),1);
    roi_location = cell(length(objects),1);
end

% generate needed values
bregma = allenCCFbregma(); % bregma position in reference data space
atlas_resolution = 0.010; % mm

% plot brain grid
ProbeColors = [1 1 1; 1 .75 0;  .3 1 1; .4 .6 .2; 1 .35 .65; .7 .7 1; .65 .4 .25; .7 .95 .3; .7 0 0; .6 0 .7; 1 .6 0];
% order of colors: {'white','gold','turquoise','fern','bubble gum','overcast sky','rawhide', 'green apple','purple','orange','red'};
fwireframe = plotBrainGrid([], [], [], black_brain); hold on;
fwireframe.InvertHardcopy = 'off';

for object_num = objects

    selected_object = objects(object_num);

    % get the object points for the currently analyzed object
    if strcmp(plane,'coronal')
        curr_objectPoints = objectPoints.pointList.pointList{selected_object,1}(:, [3 2 1]);
    elseif strcmp(plane,'sagittal')
        curr_objectPoints = objectPoints.pointList.pointList{selected_object,1}(:, [1 2 3]);
    elseif strcmp(plane,'transverse')
        curr_objectPoints = objectPoints.pointList.pointList{selected_object,1}(:, [1 3 2]);
    end

    % plot points on the wire frame brain
    figure(fwireframe); hold on
    hp = plot3(curr_objectPoints(:,1), curr_objectPoints(:,3), curr_objectPoints(:,2), '.','linewidth',2, 'color',[ProbeColors(object_num,:) .2],'markers',10);

    % use the point's position in the atlas to get the AP, DV, and ML coordinates
    ap = -(curr_objectPoints(:,1)-bregma(1))*atlas_resolution;
    dv = (curr_objectPoints(:,2)-bregma(2))*atlas_resolution;
    ml = (curr_objectPoints(:,3)-bregma(3))*atlas_resolution;

    roi_location_curr = [ap dv ml];

    % initialize array of region annotations
    roi_annotation_curr = cell(size(curr_objectPoints,1),3);

    % loop through every point to get ROI locations and region annotations
    for point = 1:size(curr_objectPoints,1)

        % find the annotation, name, and acronym of the current ROI pixel
        ann = av(curr_objectPoints(point,1),curr_objectPoints(point,2),curr_objectPoints(point,3));
        name = st.safe_name{ann};
        acr = st.acronym{ann};

        roi_annotation_curr{point,1} = ann;
        roi_annotation_curr{point,2} = name;
        roi_annotation_curr{point,3} = acr;

    end

    % save results in cell array
    if length(objects) > 1
        roi_annotation{object_num} = roi_annotation_curr;
        roi_location{object_num} = roi_location_curr;
    else
        roi_annotation = roi_annotation_curr;
        roi_location = roi_location_curr;
    end

    % display results in a table
    disp(['Clicked points for object ' num2str(selected_object)])
    roi_table = table(roi_annotation_curr(:,2),roi_annotation_curr(:,3), ...
                        roi_location_curr(:,1),roi_location_curr(:,2),roi_location_curr(:,3), roi_annotation_curr(:,1), ...
         'VariableNames', {'name', 'acronym', 'AP_location', 'DV_location', 'ML_location', 'avIndex'});
     disp(roi_table)

end

% now, use roi_location and roi_annotation for your further analyses


function varargout = processingLLCs(process, varargin)
% This function contains two different processes for manipulating the
% localized loop constraints (LLCs) to formulate a model-specific and
% objective-function-specific MILP for finding loopless flux distributions
% with a minimal number of binary variables
%
% USAGE:
%    1. Preprocess loop information after calling `addLoopLawConstraints`:
%       [solveLP, MILPproblem, loopInfo] = processingLLCs('preprocess', loopInfo, LPproblem, model, nRxns, osenseStr, MILPproblem)
%
%    2. Update the loop constraints for a specific objective vector
%       [solveLP, MILPproblem] = processingLLCs('update', loopInfo, osenseStr, MILPproblem, objVector)
%
% INPUTS:
%    loopInfo:      structure containing info about the loops, initially outputed
%                   from `addLoopLawConstraints` and updated by the current function
%    LPproblem:     original COBRA LP problem structure
%    model:         the COBRA model from which LPproblem is constructed
%    nRxns:         number of reactions in the model (default size(model.S, 2))
%    osenseStr:     optimization sense of the current problem (e.g., FBA max v_biomass, or FVA min v_PYK)
%                   'max' (defaulted) or 'min'
%    MILPproblem:   COBRA MILP problem generated from LPproblem using `addLoopLawConstraints`
%                   (default generated from LPproblem if not given in the preprocessing call)
%    objVector:     nRxns-by-1 objective vector for the current optimization problem
%                   for updating LLCs (e.g., FVA min v_PYK given fixed v_biomass)
%
% OUTPUTS:
%    solveLP:       true if solving LP is sufficient to gaurantee the objective function value
%                   is the same as solving MILP with loop constraints
%    MILPproblem:   updated MILP problem with LLCs
%    loopInfo:      updated loopInfo with info about the loops (outputed only for the preprocess call)

solveLP = true;
switch process
    case 'preprocess'
        loopInfo = preprocessLLCs(varargin{1:5});
        MILPproblem = varargin{6};
        % initial update for MILP. No specific objective reactions
        if loopInfo.alwaysLLC
            % need to solve MILP if the problem constraints or original objective function necessitate the need
            solveLP = false;
            % update bounds and rhs
            MILPproblem = updateLLCs(MILPproblem, loopInfo, []);
        end
        varargout = {solveLP, MILPproblem, loopInfo};
    case 'update'
        loopInfo = varargin{1};
        osenseStr = varargin{2};
        if isempty(osenseStr)
            osenseStr = 'max';
        end
        MILPproblem = varargin{3};
        if numel(varargin) < 4
            objVector = [];
        else
            objVector = varargin{4};
        end

        if isempty(objVector)
            % if updating but no object vector is given, just restore the original bounds
            MILPproblem = restoreOriginalBounds(MILPproblem, loopInfo.rhs0, loopInfo.var, loopInfo.BDg);
        else
            % update with a specific objective function
            [rxnID, ~, cCoeff] = find(objVector(:));
            osense = strcmp(osenseStr, 'min') - strcmp(osenseStr, 'max');
            % need to solve MILP if the problem constraints or original objective function necessitate the need or
            % the reactions being minimized has their reverse direction in loops or
            % the reactions being maximized has their forward direction in loops
            if loopInfo.alwaysLLC || any(loopInfo.rxnInLoops(rxnID(cCoeff * osense > 0), 1)) || any(loopInfo.rxnInLoops(rxnID(cCoeff * osense < 0), 2))
                solveLP = false;
                % restore the original bounds
                MILPproblem = restoreOriginalBounds(MILPproblem, loopInfo.rhs0, loopInfo.var, loopInfo.BDg);
                % find the reactions in the objective function which are being minimized and has their reverse
                % direction in loops or the reactions being maximized has their forward direction in loops
                rxnIdLLC = false(numel(rxnID));
                for j = 1:numel(rxnID)
                    if (loopInfo.rxnInLoops(rxnID(j), 1) && cCoeff(j) * osense > 0) ...
                            || (loopInfo.rxnInLoops(rxnID(j), 2) && cCoeff(j) * osense < 0)
                        rxnIdLLC(j) = true;
                    end
                end
                rxnIdLLC = rxnID(rxnIdLLC);
                % update bounds and rhs
                MILPproblem = updateLLCs(MILPproblem, loopInfo, rxnIdLLC);
            end

        end
        varargout = {solveLP, MILPproblem};
end

end

function loopInfo = preprocessLLCs(loopInfo, LPproblem, model, nRxns, osenseStr)
if nargin < 4 || isempty(nRxns)
    nRxns = size(model.S, 2);
end
if nargin < 5 || isempty(osenseStr)
    osenseStr = 'max';
end
if strcmp(osenseStr, 'min')
    model.c = -model.c;
end
rxnInLoops = loopInfo.rxnInLoops;
conComp  = loopInfo.conComp;

% determine the set of reactions for which LLCs are always required
% condition I in Prop. 2 in Chan et al., 2017
cond1 = rxnInLoops(:, 2) & model.c > 0;
% condition II in the paper in Prop. 2 in Chan et al., 2017
cond2 = rxnInLoops(:, 1) & model.c < 0;
% condition III in the paper in Prop. 2 in Chan et al., 2017
[cond3A1, cond3A2, cond3B] = deal(false(nRxns, 1));
for i = (size(model.S, 1) + 1):size(LPproblem.A, 1)
    % for constraint p with sum(a_pj * v_j) <= b_p
    if ~strcmp(LPproblem.csense(i), 'G')  % '<=' or '=' constraint
        % if reaction j has its forward direction in cycles and a_pj < 0
        cond3A1 = cond3A1 | (rxnInLoops(:, 2) & LPproblem.A(i, 1:nRxns)' < 0);
        % if reaction j has its reverse direction in cycles and a_pj > 0
        cond3A2 = cond3A2 | (rxnInLoops(:, 1) & LPproblem.A(i, 1:nRxns)' > 0);
        % if the constraint involves 2 or more reactinos or RHS < 0
        cond3B = cond3B | (nnz(LPproblem.A(i, 1:nRxns)) > 1 | LPproblem.b(i) < 0);
    end
    if ~strcmp(LPproblem.csense(i), 'L')  % '>=' or '=' constraint
        cond3A1 = cond3A1 | (rxnInLoops(:, 2) & LPproblem.A(i, 1:nRxns)' > 0);
        cond3A2 = con3A2 | (rxnInLoops(:, 1) & LPproblem.A(i, 1:nRxns)' < 0);
        cond3B = cond3B | (nnz(LPproblem.A(i, 1:nRxns)) > 1 | LPproblem.b(i) > 0);
    end
end
% reactions satisfying (3A1 or 3A2) and 3B
cond3 = (cond3A1 | cond3A2) & cond3B;
% condition III for bound constraints can be simplified as follows:
cond3 = cond3 | (model.lb > 0 & rxnInLoops(:, 2)) | (model.ub < 0 & rxnInLoops(:, 1));
% reactions that are required to be constrained by loopless constraints all the time
rxnInLoopsAlwaysOn = cond1 | cond2 | cond3;
% LLCs are always required if the set is non-empty
alwaysLLC = any(rxnInLoopsAlwaysOn);
% the corresponding set of reactions in the same connected components as
% the always-on reactions
conCompAlwaysOn = false(max(conComp), 1);
conCompAlwaysOn(conComp(rxnInLoopsAlwaysOn)) = true;
if loopInfo.printLevel
    fprintf('Reactions in internal nullspace can be divided into %d connected components.\n', max(conComp))
end

% get an initial feasible and loopless solution in case MipStart is needed
model2 = model;
model2.lb = model2.lb(1:size(model2.S, 2));
model2.ub = model2.ub(1:size(model2.S, 2));
model2.c = zeros(size(model2.S, 2), 1);
model2.b = zeros(size(model2.S, 1), 1);
sFeas = optimizeCbModel(model2, 'max', 'one');
x0 = sFeas.x;
[loopInfo.alwaysLLC, loopInfo.rxnInLoopsAlwaysOn, loopInfo.conCompAlwaysOn, loopInfo.x0] ...
    = deal(alwaysLLC, rxnInLoopsAlwaysOn, conCompAlwaysOn, x0);
end

function MILPproblemLLC = updateLLCs(MILPproblemLLC, loopInfo, rxnID)
% apply LLCs by relaxing constraints and pre-assign values to variables
if nargin < 3
    rxnID = [];
end
conCompOn = loopInfo.conCompAlwaysOn;
conCompOn(loopInfo.conComp(rxnID)) = true;

bigM = inf;
if ~loopInfo.useRxnLink
    % use connections from nullspace
    for jCon = 1:numel(loopInfo.conCompAlwaysOn)
        if ~conCompOn(jCon)
            % relax constraints not affecting optimality and feasibility
            MILPproblemLLC.b(loopInfo.con.vU(loopInfo.rxnInLoopIds(loopInfo.conComp == jCon))) = bigM;
            MILPproblemLLC.b(loopInfo.con.gU(loopInfo.rxnInLoopIds(loopInfo.conComp == jCon))) = bigM;
            MILPproblemLLC.b(loopInfo.con.vL(loopInfo.rxnInLoopIds(loopInfo.conComp == jCon))) = -bigM;
            MILPproblemLLC.b(loopInfo.con.gL(loopInfo.rxnInLoopIds(loopInfo.conComp == jCon))) = -bigM;
            % fix variables not affecting optimality and feasibility
            MILPproblemLLC.lb(loopInfo.var.g(loopInfo.rxnInLoopIds(loopInfo.conComp == jCon))) = 0;
            MILPproblemLLC.ub(loopInfo.var.g(loopInfo.rxnInLoopIds(loopInfo.conComp == jCon))) = 0;
            MILPproblemLLC.ub(loopInfo.var.z(loopInfo.rxnInLoopIds(loopInfo.conComp == jCon))) = 0;
        end
    end
else
    % use connections from EFMs
    rxnOn = loopInfo.rxnInLoopsAlwaysOn;
    rxnOn(rxnID) = true;

    % reactions in cycles not sharing EFMs with the current rxns and
    % not being one of the reactions required to have no flux through cycles
    id = ~any(loopInfo.rxnLink(rxnOn, :), 1)' & any(loopInfo.rxnInLoops, 2);
    % the loop constraints on them are relaxed
    MILPproblemLLC.b(loopInfo.con.vU(loopInfo.rxnInLoopIds(id))) = bigM;
    MILPproblemLLC.b(loopInfo.con.gU(loopInfo.rxnInLoopIds(id))) = bigM;
    MILPproblemLLC.b(loopInfo.con.vL(loopInfo.rxnInLoopIds(id))) = -bigM;
    MILPproblemLLC.b(loopInfo.con.gL(loopInfo.rxnInLoopIds(id))) = -bigM;

    % pre-determine variables not connected to the reaction for FVA
    % except reactions required to be always constrained
    rxnKeep = loopInfo.conComp == 0;
    for jCon = 1:numel(conCompOn)
        if conCompOn(jCon)
            rxnKeep(loopInfo.conComp == jCon) = true;
        end
    end
    MILPproblemLLC.lb(loopInfo.var.g(loopInfo.rxnInLoopIds(~rxnKeep))) = 0;
    MILPproblemLLC.ub(loopInfo.var.g(loopInfo.rxnInLoopIds(~rxnKeep))) = 0;
    MILPproblemLLC.ub(loopInfo.var.z(loopInfo.rxnInLoopIds(~rxnKeep))) = 0;
end
end

function MILPproblemLLC = restoreOriginalBounds(MILPproblemLLC, rhs0, varInd, BDg)
    MILPproblemLLC.b = rhs0;
    MILPproblemLLC.ub(varInd.z) = 1;
    MILPproblemLLC.ub(varInd.g) = BDg;
    MILPproblemLLC.lb(varInd.g) = -BDg;
end


classdef LvdDataPoint < AbstractGeometricPoint
    %LvdDataPoint Summary of this class goes here
    %   Detailed explanation goes here

    properties
        allTimes cell = {};
        allRVVect cell = {};
        allFrames(1,:) AbstractReferenceFrame
        allGIs cell = {};

        name(1,:) char = 'New Point';

        inputLvdData LvdData
        lvdData LvdData

        %marker
        markerColor(1,1) ColorSpecEnum = ColorSpecEnum.Red;
        markerShape(1,1) MarkerStyleEnum = MarkerStyleEnum.RightTriangle;

        %track line
        plotTrkLine(1,1) logical = true;
        trkLineColor(1,1) ColorSpecEnum = ColorSpecEnum.Black;
        trkLineSpec(1,1) LineSpecEnum = LineSpecEnum.DottedLine;
    end

    methods
        function obj = LvdDataPoint(inputLvdData, lvdData, name)
            arguments
                inputLvdData(1,1) LvdData %the lvd data that contains the trajectory to be loaded
                lvdData(1,1) LvdData %the lvd data for the current case/scenario/mission
                name(1,:) char %the name of the point
            end

            obj.name = name;
            obj.lvdData = lvdData;

            obj.loadLvdData(inputLvdData, lvdData);
        end

        function tf = hasValidLvdData(obj)
            tf = true;

            if(isempty(obj.inputLvdData) || ...
               obj.inputLvdData == obj.lvdData)
                tf = false;

                return;
            end
        end

        function cartElems = getPositionAtTime(obj, inputTimes, ~, inFrame)
            cartElems = obj.getCartElemForTime(inputTimes, inFrame);
        end

        function loadLvdData(obj, inputLvdData, lvdData)
            obj.inputLvdData = inputLvdData;

            stateLog = inputLvdData.script.executeScript(false, inputLvdData.script.getEventForInd(1), true, false, false, false);

            obj.allTimes = {};
            obj.allRVVect = {};
            obj.allFrames = AbstractReferenceFrame.empty(1,0);
            obj.allGIs = {};

            for(i=1:inputLvdData.script.getTotalNumOfEvents()) %#ok<*NO4LP>
                evt = inputLvdData.script.getEventForInd(i);
                entries = stateLog.getAllStateLogEntriesForEvent(evt);

                [~,ia,~] = unique([entries.time], 'sorted');
                entries = entries(ia);

                if(numel(entries) > 2)
                    useFrame = entries(1).centralBody.getBodyCenteredInertialFrame();

                    times = [];
                    rvVects = [];
                    for(j=1:length(entries))
                        entry = entries(j);
                        maEntry = entry.getMAFormattedStateLogMatrix(true);

                        time = maEntry(1);
                        rVect = maEntry(2:4)';
                        vVect = maEntry(5:7)';

                        bodyInfo = lvdData.celBodyData.getBodyInfoById(maEntry(8));
                        frame = bodyInfo.getBodyCenteredInertialFrame();

                        ce = CartesianElementSet(time, rVect(:), vVect(:), frame);
                        ce = ce.convertToFrame(useFrame, true);

                        rvVect = [ce.rVect; ce.vVect];

                        times(j) = time; %#ok<AGROW>
                        rvVects(:,j) = rvVect; %#ok<AGROW>
                    end

                    obj.allTimes{end+1} = times;
                    obj.allRVVect{end+1} = rvVects;
                    obj.allFrames(end+1) = useFrame;
                    obj.allGIs{end+1} = griddedInterpolant(times,[rvVects;times]', "makima", "nearest");
                end
            end
        end

        function name = getName(obj)
            name = obj.name;
        end

        function setName(obj, name)
            obj.name = name;
        end

        function listboxStr = getListboxStr(obj)
            listboxStr = sprintf('%s (LVD Trajectory)', obj.getName());
        end

        function useTf = openEditDialog(obj, ~)
            output = AppDesignerGUIOutput({false});
            lvd_EditLvdTrajectoryPointGUI_App(obj, output, obj.lvdData);
            useTf = output.output{1};
        end

        function tf = isVehDependent(obj) %#ok<MANU>
            tf = false;
        end

        function tf = canBePlotted(obj) %#ok<MANU>
            tf = true;
        end

        function bodyInfo = getOriginBody(obj)
            bodyInfo = obj.bodyInfo;
        end

        function tf = usesGroundObj(~, ~)
            tf = false;
        end

        function tf = usesGeometricPoint(~, ~)
            tf = false;
        end

        function tf = usesGeometricVector(~, ~)
            tf = false;
        end

        function tf = usesGeometricCoordSys(~, ~)
            tf = false;
        end

        function tf = usesGeometricRefFrame(~, ~)
            tf = false;
        end

        function tf = usesGeometricAngle(~, ~)
            tf = false;
        end

        function tf = usesGeometricPlane(~, ~)
            tf = false;
        end

        function tf = isInUse(obj, lvdData)
            tf = lvdData.usesGeometricPoint(obj);
        end
    end

    methods(Access=private)
        function cartElem = getCartElemForTime(obj, qTimes, inFrame)
            fillerCe = CartesianElementSet(0, [0;0;0], [0;0;0], inFrame);
            cartElem = repmat(fillerCe, [1, numel(qTimes)]);

            for(i=1:length(obj.allTimes))
                times = obj.allTimes{i};
                minTime = min(times);
                maxTime = max(times);

                bool = qTimes >= minTime & qTimes <= maxTime;
                if(any(bool))
                    subQTimes = qTimes(bool);
                    gi = obj.allGIs{i};
                    frame = obj.allFrames(i);

                    rvVects = gi(subQTimes)';
                    subCe = CartesianElementSet(subQTimes, rvVects(1:3,:), rvVects(4:6,:), frame);
                    subCe = convertToFrame(subCe, inFrame, true);
                    cartElem(bool) = subCe;
                end
            end

            bool = cartElem == fillerCe;
            if(any(bool))
                outQTimes = qTimes(bool);

                for(i=1:length(obj.allTimes))
                    times = obj.allTimes{i};
                    minTime = min(times);
                    maxTime = max(times);

                    distToLb = abs(outQTimes - minTime);
                    distToUb = abs(outQTimes - maxTime);
                    distToBnd(:,i) = min([distToLb(:), distToUb(:)], [], 2); %#ok<AGROW>
                end

                boolInds = find(bool);

                [~,I] = min(distToBnd, [], 2);
                for(i=1:length(I))
                    qTime = outQTimes(i);
                    Ii = I(i);

                    gi = obj.allGIs{Ii};
                    frame = obj.allFrames(Ii);

                    rvVects = gi(qTime)';
                    rVects = rvVects(1:3,:);
                    vVects = rvVects(4:6,:);
                    nearestTimes = rvVects(7,:);
                    subCe = CartesianElementSet(nearestTimes, rVects, vVects, frame);
                    subCe = convertToFrame(subCe, inFrame, true);
                    cartElem(boolInds(i)) = subCe;
                end
            end
        end
    end
end


function e2s2_loadAndEvaluate(varargin)
% e2s2_loadAndEvaluate(varargin)
%
% Evaluate a network snapshot on a given dataset.
%
% Copyright by Holger Caesar, 2016

% Initial settings
p = inputParser;
addParameter(p, 'dataset', SiftFlowDatasetMC());
addParameter(p, 'weaklySupervised', false);
addParameter(p, 'run', 28);
addParameter(p, 'exp', 25);
addParameter(p, 'epochs', [1, 5:5:30]);
addParameter(p, 'gpus', 1);
addParameter(p, 'subset', 'test');
addParameter(p, 'plotStats', true);
addParameter(p, 'minSize', []); % typically 100
addParameter(p, 'maxSizeRel', []);
addParameter(p, 'doCache', true);
addParameter(p, 'storeOutputMaps', false);
addParameter(p, 'limitImageCount', Inf);
addParameter(p, 'subsamplePosRange', []);
addParameter(p, 'testColorSpace', []);
parse(p, varargin{:});

dataset = p.Results.dataset;
weaklySupervised = p.Results.weaklySupervised;
run = p.Results.run;
exp = p.Results.exp;
epochs = p.Results.epochs;
gpus = p.Results.gpus;
subset = p.Results.subset;
plotStats = p.Results.plotStats;
minSize = p.Results.minSize;
maxSizeRel = p.Results.maxSizeRel;
doCache = p.Results.doCache;
storeOutputMaps = p.Results.storeOutputMaps;
limitImageCount = p.Results.limitImageCount;
subsamplePosRange = p.Results.subsamplePosRange;
testColorSpace = p.Results.testColorSpace;

% Settings
plotStats = plotStats && strcmp(subset, 'test') && numel(epochs) > 1;
stats = cell(numel(epochs), 1);
if ~isempty(minSize) || ~isempty(maxSizeRel) || ~isinf(limitImageCount),
    fprintf('Warning: Cannot cache results due to custom (blob size or limitImageCount) settings!\n');
    doCache = false;
end

% Create paths
global glFeaturesFolder;
if weaklySupervised,
    wsStr = 'ws';
else
    wsStr = '';
end
outputName = sprintf('%s_e2s2%s_run%d_exp%d', dataset.name, wsStr, run, exp);
netFolder = fullfile(glFeaturesFolder, 'CNN-Models', 'E2S2', dataset.name, sprintf('Run%d', run), outputName);

%%% Load and set netOpts
netOptsPath = fullfile(netFolder, 'net-opts.mat');
netOptsStruct = load(netOptsPath, 'imdb', 'nnOpts');
netOptsStruct.nnOpts.gpus = gpus;
netOptsStruct.nnOpts.expDir = netFolder;

% Disable conversion from test to train
netOptsStruct.nnOpts.convertToTrain = false;

% Update the dataset in the imdb to avoid a nasty bug due to changed dataset classes
netOptsStruct.imdb.dataset = dataset;

% Set testing options to restrict regions by size
if ~isempty(maxSizeRel),
    netOptsStruct.nnOpts.misc.testOpts.maxSizeRel = maxSizeRel;
end
if ~isempty(minSize),
    netOptsStruct.nnOpts.misc.testOpts.minSize = minSize;
end
if ~isempty(subsamplePosRange),
    netOptsStruct.nnOpts.misc.testOpts.subsamplePosRange = subsamplePosRange;
end
if ~isempty(testColorSpace)
    netOptsStruct.nnOpts.misc.testOpts.testColorSpace = testColorSpace;
end

for epochIdx = 1 : numel(epochs),
    epoch = epochs(epochIdx);
    netPath = fullfile(netFolder, sprintf('net-epoch-%d.mat', epoch));

    % Load net
    netIn = load(netPath, 'net', 'stats');

    % Create network
    nnClass = E2S2NN(netIn, netOptsStruct.imdb, netOptsStruct.nnOpts);

    % Test network
    stats{epochIdx} = nnClass.testOnSet('subset', subset, 'doCache', doCache, 'limitImageCount', limitImageCount, 'storeOutputMaps', storeOutputMaps);
    fprintf('Displaying stats for epoch %d of exp %s...\n', epoch, outputName);
    disp(stats{epochIdx});
end

% Create a plot of the above stats
if plotStats,
    trainLoss = cell2mat({nnClass.stats.train.objective});
    valLoss   = cell2mat({nnClass.stats.val.objective});
    paccs = cellfun(@(s) s.pacc, stats);
    maccs = cellfun(@(s) s.macc, stats);
    mius  = cellfun(@(s) s.miu, stats);

    figure(1); clf;

    subplot(2, 1, 1);
    hold on;
    plot(1:numel(trainLoss), trainLoss);
    plot(1:numel(valLoss),     valLoss);
    legend({'train', 'val'});
    xlabel('epoch');
    ylabel('loss');
    ax = gca;
    axis([0, numel(trainLoss), ax.YLim]);
    grid on;

    subplot(2, 1, 2);
    hold on;
    plot(epochs, paccs);
    plot(epochs, maccs);
    plot(epochs, mius);
    legend({'Pix. Acc. test', 'Class Acc. test', 'Mean IU test'}, 'Location', 'SouthEast');
    xlabel('epoch');
    ylabel('accuracy');
    ax = gca;
    axis([0, numel(trainLoss), ax.YLim]);
    grid on;

    plotPath = fullfile(netFolder, 'net-test.pdf');
    if exist(plotPath, 'file'),
        error('Error: plotPath already exists: %s', plotPath);
    end
    print(1, plotPath, '-dpdf'); %#ok<MCPRT>
end


% ----------------------------------------------------------------------------
% function hfssCreateRelativeCS(fid, Name, Origin, Units)
%
% Description :
% -------------
% Create a relative Coordinate System at "origin" point relative to the
% Global Coordinate System.
%
% Parameters :
% ------------
% fid     - file identifier of the HFSS script file.
% Name    -
% Origin  - point relative to the Global Coordinate System where the
%           Relative Coordinate System will be created.
% Units   - units of the points (specify using either 'in', 'mm', 'meter'
%           or anything else defined in HFSS).
%
% Note :
% ------
% Use this function to facilitate drawing stuff. To return to use
% another CS (i.e. the Global CS), you can use the function hfssSetWCS
%
% Example :
% ---------
% fid = fopen('myantenna.vbs', 'wt');
% ...
% hfssInsertDesign(fid, 'Dipole_SingleElement');
% ----------------------------------------------------------------------------

% ----------------------------------------------------------------------------
% Written by Daniel R. Prado
% danysan@gmail.com / drprado@tsc.uniovi.es
% 20 September 2012
% ----------------------------------------------------------------------------

function hfssCreateRelativeCS(fid, Name, Origin, Units)

% arguments processor.
if (nargin < 4)
	error('Insufficient number of arguments !');
end

% Preamble.
fprintf(fid, '\n');
fprintf(fid, 'oEditor.CreateRelativeCS _\n');

% CS Parameters
fprintf(fid, 'Array("NAME:RelativeCSParameters", _\n');
fprintf(fid, '"OriginX:=", "%.4f%s", _\n', Origin(1), Units);
fprintf(fid, '"OriginY:=", "%.4f%s", _\n', Origin(2), Units);
fprintf(fid, '"OriginZ:=", "%.4f%s", _\n', Origin(3), Units);
fprintf(fid, '"XAxisXvec:=", "1%s", _\n', Units);
fprintf(fid, '"XAxisYvec:=", "0%s", _\n', Units);
fprintf(fid, '"XAxisZvec:=", "0%s", _\n', Units);
fprintf(fid, '"YAxisXvec:=", "0%s", _\n', Units);
fprintf(fid, '"YAxisYvec:=", "1%s", _\n', Units);
fprintf(fid, '"YAxisZvec:=", "0%s"), _\n', Units);

% CS Attributes
fprintf(fid, 'Array("NAME:Attributes", _\n');
fprintf(fid, '"Name:=", "%s")\n', Name);


hydro = struct();

hydro = readNEMOH(hydro,'../Coer_Comp/');
% hydro = readWAMIT(hydro,'../../WAMIT/Coer_Comp/coer_comp.out',[]);
% hydro = combineBEM(hydro); % Compare to WAMIT
hydro = radiationIRF(hydro,10,[],[],[],[]);
hydro = radiationIRFSS(hydro,[],[]);
hydro = excitationIRF(hydro,10,[],[],[],[]);
writeBEMIOH5(hydro)
plotBEMIO(hydro)


function [region_obj, results_table, varargout] = table(w, varargin)
% Print a table of all regions in a thresholded image. Return a Matlab table object
% and labeled region object. Used with fmri_data, statistic_image, and
% image_vector objects.
%
% This function has two "modes":
% - Without an optional atlas object input, it uses region.table to generate a table based
% on contiguous clusters (separated by those with positive and negative peak values)
%
% - With an optional atlas object input, it uses image_vector.subdivide_by_atlas to
% generate a table based on atlas regions covered by the input image. The table has
% one row per atlas region. The region object is also divided by the atlas,
% so one large contiguous blob in the image may cover multiple rows with
% different labeled regions.
%
% :Usage:
% ::
%
%    [poscl, negcl] = table(cl, [optional inputs])
%
% - By default, region.table() separates clusters into subregions with positive
%   and negative values. Thus, the number of rows may not match the original number of regions.
%   To turn this feature off, use 'nosep'.
% - By default, region.table() re-sorts the regions to group the table rows by macro-scale brain
%   structures (cortex, basal ganglia, etc.). So the ordering in the table may not match
%   the original region object. To turn this feature off, use 'nosort'.
%
% :Optional inputs:
%
%   **atlas_obj**
%       Any atlas-class object, e.g., loaded by load_atlas().
%
%   **k:**
%        Print only regions with k or more contiguous voxels
%
%   **nosep:**
%        do not separate cl with pos and neg effects based on peak in .val
%
%   **names:**
%        name clusters manually before printing to table and output; saves in .shorttitle field
%
%   **forcenames:**
%        force manual naming of cl by removing existing names in .shorttitle field
%
%   **nosort:**
%        Do not sort rows by network/brain lobe [default is to sort]
%
%   **legacy:**
%        force manual naming of cl by removing existing names in .shorttitle field
%
%   **nolegend:**
%        omit table legend
%
% :Outputs:
%
%   Returns region objects for cl with pos and neg effects
%   - autolabeled if Neuroimaging_Pattern_Masks and atlas tools are available on Matlab path
%   - limited by size if entered
%   - manually named if requested (see optional inputs)
%
% ..
%     Author and copyright information:
%
%     Copyright (C) 2011  Tor Wager
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
% ..
% :Examples:
% -------------------------------------------------------------------------
% ::
% Example 1:
% % Complete group analysis of a standard dataset
% % Do analysis and prep results region object:
%
%   img_obj = load_image_set('emotionreg');         % Load a dataset
%   t = ttest(img_obj, .005, 'unc');                % Do a group t-test
%   t = threshold(t, .005, 'unc', 'k', 10);         % Re-threshold with extent threshold of 10 contiguous voxels
%   r = region(t);                                  % Turn t-map into a region object with one element per contig region
%
%   Label regions and print a table:
%   [r, region_table, table_legend_text] = autolabel_regions_using_atlas(r);
%                                                   % Label regions. Can be skipped because 'table' below attempts to do this automatically
%   table(r);                                       % Print a table of results using new region names
%
%   [rpos, rneg] = table(r);                        % Print and table and return region object separated into regions with positive vs. negative statistic values (from .Z field)

% ..
%    Programmers' notes:
%    List dates and changes here, and author of changes
% ..
%    July 2018:  Autolabel update and "new 2018 version", Tor Wager. Also added legend text.

n_cols = 140;                       % 140 good for HTML reports
sep_str = repmat('_', 1, n_cols);   % see textwrap

k = 0;
dosep = true;
donames = false;        % name clusters before printing to table and output; saves in .shorttitle field (legacy only)
forcenames = false;     % force naming of cl by removing existing names in .shorttitle field (legacy only)
dolegacy = false;
dosortrows = true;          % sort rows by area
dolegend = true;

for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}

            % functional commands
            case {'k', 'maxsize'}, k = varargin{i+1};
            case 'nosep', dosep = 0;
            case {'names', 'name', 'donames'}, donames = true;
            case 'forcenames', forcenames = true;

            case {'nosort', 'nosortrows'}, dosortrows = false;

            case {'legacy', 'dolegacy'}, dolegacy = true;

            case 'nolegend', dolegend = false;

            otherwise, warning(['Unknown input string option:' varargin{i}]);
        end
    end
end

[poscl, negcl, results_table] = table(cl, varargin)

[w_atlas, w_region_obj] = subdivide_by_atlas(w, atl);

% Make a table of labeled regions with significant voxels in the weight map

[region_obj, region_table] = autolabel_regions_using_atlas(w_region_obj, atl);
region_table = region_table(:, 1:3);


function obj2 = fft( obj1, varargin )

obj2 = unitaryopp(@fft, obj1, varargin{:});


function [x,fval,exitflag,info] = opti_filtersd(fun,grad,x0,lb,ub,nlcon,nljac,nljacstr,cl,cu,opts)
%OPTI_FILTERSD Solve a NLP using FILTERSD
%
%   min f(x)       subject to:    cl <= nlcon(x) <= cu
%    x                            lb <= x <= ub
%
%   x = opti_filtersd(fun,grad,x0) solves a NLP where fun is the objective
%   function, grad is the gradient of the objective and x0 is a starting
%   guess.
%
%   x = opti_filtersd(fun,grad,x0,lb,ub) solves subject to decision
%   variable bounds lb <= x <= ub. Infinite bounds are OK.
%
%   x = opti_filtersd(fun,x0,lb,ub,nlcon,nljac,[],cl,cu) solves subject to
%   the nonlinear inequality constraints cl <= nlcon(x) <= cu, and nljac
%   returns a DENSE Jacobian of the constraints.
%
%   x = opti_filtersd(fun,x0,lb,ub,nlcon,nljac,nljacstr,cl,cu) solves as
%   above, except nljac returns a SPARSE Jacobian of the constraints, and
%   nljacstr supplies a sparse matrix of ALL possible non-zero locations
%   within the Jacobian.
%
%   x = opti_filtersd(fun,...,cu,opts) uses opts to pass optiset options to
%   the solver.
%
%   [x,fval,exitflag,info] = opti_filtersd(...) returns the objective value
%   at the solution, together with the solver exitflag, and an information
%   structure.
%
%   THIS IS A WRAPPER FOR FILTERSD
%   See supplied Eclipse Public License

%   Copyright (C) 2013 Jonathan Currie (IPL)

if(nargin < 11), opts = optiset; else opts = optiset(opts); end
if(nargin < 10), cu = []; end
if(nargin < 9),  cl = []; end
if(nargin < 8),  nljacstr = []; end
if(nargin < 7),  nljac = []; end
if(nargin < 6),  nlcon = []; end
if(nargin < 5),  ub = []; end
if(nargin < 4),  lb = []; end
if(nargin < 3),  error('FILTERSD requires at least 3 arguments'); end

%Default is not sparse
sp = false;

%Determine we have a sparse problem
if(~isempty(nljacstr))
    sp = true; %must be sparse if we passed an arg here
    if(~isa(nljacstr,'function_handle'))
        error('Jacobian Structure must be a function handle');
    end
end

%Check we have a valid x0
if(isempty(x0) || any(isnan(x0)))
    error('FILTERSD requires an initial guess, x0!');
end

%Setup display level
opts.display = dispLevel(opts.display);
opts.optiver = optiver;

t = tic;
if(sp)
    % Run FILTERSD [Sparse]
    jacstr = nljacstr(); %filtersd just requires the matrix
    [x, fval, exitflag, stats, lambda] = filtersdsp(fun,grad,x0,lb,ub,nlcon,nljac,jacstr,cl,cu,opts);
else
    % Run FILTERSD [Dense]
    [x, fval, exitflag, stats, lambda] = filtersd(fun,grad,x0,lb,ub,nlcon,nljac,cl,cu,opts);
end

%Collect Results
info.Iterations = stats.niter;
info.FuncEvals = stats.nfval;
info.GradEvals = stats.ngval;
info.Time = toc(t);
if(sp)
    info.Algorithm = 'FILTERSD: Nonlinear Optimization using the Filter Search & Trust Region [Sparse]';
else
    info.Algorithm = 'FILTERSD: Nonlinear Optimization using the Filter Search & Trust Region [Dense]';
end

switch(exitflag)
    case 0
        info.Status = 'Locally Optimal';
        exitflag = 1;
    case {5,101,102}
        info.Status = 'Exceeded Iterations / Function Evaluations / Time';
        exitflag = 0;
    case {1,2,4,7,8}
        info.Status = 'Initialization Error';
        exitflag = -2;
    case {3,6}
        info.Status = 'Infeasible / Inconsistent Constraints';
        exitflag = -1;
    case {9,10}
        info.Status = 'LCP Solver Error';
        exitflag = -3;
    case 105
        info.Status = 'User Exited';
        exitflag = -5;
    otherwise
        info.Status = 'FILTERSD Error';
end

info.Lambda = lambda;


function [im, scale] = readImage(imagePath)
% READIMAGE   Read and standardize image
%    [IM, SCALE] = READIMAGE(IMAGEPATH) reads the specified image file,
%    converts the result to SINGLE class, and rescales the image
%    to have a maximum height of 480 pixels, returing the corresponding
%    scaling factor SCALE.
%
%    READIMAGE(IM) where IM is already an image applies only the
%    standardization to it.

% Author: Andrea Vedaldi

% Copyright (C) 2013 Andrea Vedaldi
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if ischar(imagePath)
  try
    im = imread(imagePath) ;
  catch
    error('Corrupted image %s', imagePath) ;
  end
else
  im = imagePath ;
end

im = im2single(im) ;

scale = 1 ;
if (size(im,1) > 480)
  scale = 480 / size(im,1) ;
  im = imresize(im, scale) ;
  im = min(max(im,0),1) ;
end


function out_fwrite_edf(sFile, sfid, SamplesBounds, ChannelsRange, F)
% OUT_FWRITE_EDF: Write a block of recordings from a EDF file.

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Martin Cousineau, 2017
%          Francois Tadel, 2019

% ===== PARSE INPUTS =====
[nSignals, nSamples] = size(F);
if isempty(SamplesBounds)
    SamplesBounds = [0, nSamples];
end
if isempty(ChannelsRange)
    ChannelsRange = [1, nSignals];
end

fseek(sfid, 0, 'eof');

% Get the gains of the channels for all the non-Annotation channels
iChanGain = setdiff(1:length(sFile.header.signal), sFile.header.annotchan);
% Apply channel gains before converting to integer
chgain = [sFile.header.signal(iChanGain).unit_gain] ./ ...
            ([sFile.header.signal(iChanGain).physical_max] - [sFile.header.signal(iChanGain).physical_min]) .* ...
            ([sFile.header.signal(iChanGain).digital_max]  - [sFile.header.signal(iChanGain).digital_min]);
F = bst_bsxfun(@times, F, chgain');

% Convert to 2-byte integer in 2's complement
F = int16(F);
negF = F < 0;
F(negF) = bitcmp(abs(F(negF))) + 1;

% Prepare annotations if any.
if sFile.header.annotchan >= 0
    annotations    = 1;
    nAnnots        = numel(sFile.header.annotations);
    nSamplesReal   = round((sFile.prop.times(2) - sFile.prop.times(1)) .* sFile.prop.sfreq);
    nSamplesAnnots = sFile.header.signal(sFile.header.annotchan).nsamples;
    annotThreshold = floor((1:nAnnots) / nAnnots * nSamplesReal);
    annotBounds    = [0, 0];

    % Insert annotation in this record only if it contains the required
    % cutoff sample threshold
    for iThr = 1:nAnnots
        if annotThreshold(iThr) >= SamplesBounds(1) && annotThreshold(iThr) <= SamplesBounds(2)
            if annotBounds(1) < 1
                annotBounds(1) = iThr;
            end
            annotBounds(2) = iThr;
        end
    end

    if annotBounds(2) < 1
        annotsList = [];
    else
        annotsList = sFile.header.annotations(annotBounds(1) : annotBounds(2));
    end

    nAnnots     = numel(annotsList);
    nextAnnot   = 1;
else
    annotations = 0;
end

% Write to file record per record
nSamplesPerRecord = sFile.prop.sfreq * sFile.header.reclen;
nRecords          = ceil((SamplesBounds(2) - SamplesBounds(1)) / nSamplesPerRecord);
ncount            = 0;
bounds            = [1, nSamplesPerRecord];
timeOffset        = SamplesBounds(1) / sFile.prop.sfreq;

for iRec = 1:nRecords
    % Special case when we don't have enough data to fill the last record
    if bounds(2) > nSamples
        if iRec ~= nRecords
            error('Ran out of data before last record.');
        end
        writeZeros = bounds(2) - nSamples;
        bounds(2)  = nSamples;
    else
        writeZeros = 0;
    end

    % Write data
    for iSig = ChannelsRange(1):ChannelsRange(2)
        ncount = ncount + fwrite(sfid, F(iSig, floor(bounds(1)):floor(bounds(2))), 'int16');

        % Fill rest of the record with 0s if required
        if writeZeros
            fwrite(sfid, zeros(writeZeros, 1), 'int16');
        end
    end

    % Write annotations if any, split by records
    if annotations
        bytesLeft = nSamplesAnnots * 2;

        % The first annotation specifies the time offset
        bytesLeft = bytesLeft - fprintf(sfid, '+%f%c%c%c', timeOffset, char(20), char(20), char(0));

        % Write as many annotations as possible in current record
        while nextAnnot <= nAnnots && bytesLeft >= length(annotsList{nextAnnot})
            bytesLeft = bytesLeft - fprintf(sfid, '%s', annotsList{nextAnnot});
            nextAnnot = nextAnnot + 1;
        end

        % Fill remaining of record with 0-bytes.
        fprintf(sfid, '%s', repmat(char(0), 1, bytesLeft));
    end

    % Get ready for next record
    bounds     = bounds + nSamplesPerRecord;
    timeOffset = timeOffset + sFile.header.reclen;
end

% Check number of values written
if (ncount ~= numel(F))
    error('Error writing data to file.');
end


function ExportAedat2Frames(aedat)

%{
This function exports data to a .aedat file.
The .aedat file format is documented here:

http://inilabs.com/support/software/fileformat/
%}

dbstop if error

if ~exist('aedat', 'var')
	error('Missing input')
end

if ~isfield(aedat, 'data')
	disp('No data to export')
    return
end

% Create the file
if ~isfield(aedat.exportParams, 'filePath')
    error('Missing file path and name')
end

f = fopen(aedat.exportParams.filePath, 'w', 'b');

% Simple - events only - assume DAVIS

% CRLF \r\n is needed to not break header parsing in jAER
fprintf(f,'#!AER-DAT2.0\r\n');
fprintf(f,'# This is a raw AE data file created by an export function in the AedatTools library\r\n');
fprintf(f,'# Data format is int32 address, int32 timestamp (8 bytes total), repeated for each event\r\n');
fprintf(f,'# Timestamps tick is 1 us\r\n');
% Put the source in - use an override if it has been given
if isfield(aedat.exportParams, 'source')
    fprintf(f,['# AEChip: ' aedat.exportParams.source '\r\n']);
else
    fprintf(f,['# AEChip: ' aedat.info.source '\r\n']);
end
fprintf(f,'# End of ASCII Header\r\n');

% DAVIS
% In the 32-bit address (1-based):
% Bit-32   Bit-11   Meaning
% 1        0        APS sample
% 1        1        IMU sample
% 0        1        Special event
% 0        0        Polarity event

yShiftBits = 22;
xShiftBits = 12;
% frameShiftBits = 0;
frameFlagShiftBits = 31;
signalShiftBits = 9;

frameData = aedat.data.frame;

numFrames = frameData.numEvents;
xDim = aedat.info.deviceAddressSpace(1);
yDim = aedat.info.deviceAddressSpace(2);
numPixels = xDim * yDim;

% Allocate horizontal vectors to hold output data.
% Why are the vectors that big? The factor of 2 is because
% we insert dummy 'reset' frames prior to each frame.
samples = uint32(zeros(1, 2 * numFrames * numPixels));
timeStamps = uint32(zeros(1, 2 * numFrames * numPixels));

% The output vector is twice as big again because samples and timeStamps
% will be interspersed in the 'output' vector.
output = uint32(zeros(1, 2 * 2 * numFrames * numPixels));
y = repmat(uint32(yDim - 1 : -1 : 0), 1, xDim * numFrames * 2);
x = repmat(uint32(xDim - 1 : -1 : 0), yDim, numFrames * 2);
x = x(:);
x = x';
% in bit 11 (1-based) 1 means signal read and 0 means reset read.
signalFlag = repmat([zeros(1, numPixels, 'uint32')  ones(1, numPixels, 'uint32') * 2^10], 1, numFrames);
% The last event mask is synonymous with the sample from x=0 y=0; data is
% therefore ordered backwards.
for frameIndex = 1 : numFrames
    samplesTemp = frameData.samples{frameIndex}(:);
    samplesTemp = samplesTemp(end : -1 : 1);
    samples((frameIndex * 2 - 1) * numPixels + 1 : frameIndex * 2 * numPixels) ...
        = samplesTemp ;
    timeStamps((frameIndex - 1) * 2 * numPixels + 1 : frameIndex * 2 * numPixels) ...
        = frameData.timeStampStart(frameIndex);
end
frameFlag = uint32(ones(1, numFrames * 2 * numPixels) * 2 ^ frameFlagShiftBits);
y = y * uint32(2 ^ yShiftBits);
x = x * uint32(2 ^ xShiftBits);
% samples should now be in the range 0-1023 (10-bit).
% subtract samples from 1023. This has the effect of leaving all the reset
% frame samples at 1023 - the highest value, against which the signal frames
% will later be subtracted.
samples = 1023 - samples;

output(1:2:end) = frameFlag + y + x + signalFlag + samples;
output(2:2:end) = timeStamps; % set even elements to timestamps

% write addresses and timestamps
count=fwrite(f, output, 'uint32', 0, 'b')/2; % write 4 byte data
fclose(f);
fprintf('wrote %d events to %s\n', count, aedat.exportParams.filePath);


run('matconvnet/matlab/vl_setupnn.m');
addpath('util/');

outDir = 'result'; % output directory
data = 'Set5';     % test data (directory) which is in data folder
SF = 2;            % test scale factors. can be 2, 3 or 4
outRoute = fullfile(outDir, data, ['VDSR_x',num2str(SF)]);

if ~exist(outRoute, 'dir')
    mkdir(outRoute);
end

%VDSR(data, SF, 'VDSR.mat', outRoute);
VDSR(data, SF, 'VDSR_CPUonly_SupER.mat', outRoute); % MAGI-ADAPT


function a = subsasgn( a,s,b )
%SUBSASGN for nested cells
%
% (c) Thomas Kuestner
% ---------------------------------------------------------------------

if(length(s) > 1)
    posCell = size(s(1).subs,2);
    posCell = s(1).subs{1,posCell};
    posMat = cell2mat(s(2).subs);
    [out,idx] = flattenCellMatrix(a.data);
    out{posCell}(sub2ind(size(out{posCell}),posMat(1),posMat(2),posMat(3))) = b;
    a.data = reconFlatCellMatrix(out,idx);

else
    if(strcmp(s.type,'{}'))
        pos = size(s.subs,2);
        pos = s.subs{1,pos};
        [out,idx] = flattenCellMatrix(a.data);
        out{pos} = b;
        a.data = reconFlatCellMatrix(out,idx);

    else
        pos = size(s.subs,2);
        pos = s.subs{1,pos};
        % a = a.data;
        % meta = a.meta;
        if(isa(b,'TRAFO'))
            if(size(s.subs,2) == 5)
                a.data(pos,:) = b.data; % for kernelImg (due to 5D dataset => cha-cha)
                if iscell(b.meta) ~= 1
                    a.meta = b.meta; %changed for curvelab!
                else
                    a.meta(pos,:) = b.meta;
                end
            else
                a.data(1,pos) = b.data;
                if(~iscell(b.meta) || length(b.meta) > 1)
                    a.meta = b.meta; %changed for curvelab!
                else
                    a.meta(1,pos) = b.meta;
                end
            end
        % a = TRAFO(a,meta);
        else
            [out,idx] = flattenCellMatrix(a.data(1,pos));
            for i=1:length(out)
                out{i}(:) = b;
            end
            a.data(1,pos) = reconFlatCellMatrix(out,idx);
        end
    end
end

end


function outputImage = plus(this, otherImage)
% Adds data of other image to this one and writes output in new image
%
% NOTE: If voxel dimensions of 2nd image do not match - or a scalar is
% given as 2nd argument - data in the 2nd argument is automatically
% replicated to match this image geometry.
%
%
%   Y = MrImage()
%   outputImage = plus(Y, otherImage, ...
%   functionHandle)
%
% This is a method of class MrImage.
%
%
% IN
%   otherImage              image that will be added to this one
%
% OUT
%   outputImage             new MrImage, sum of this and otherImage
%
% EXAMPLE
%
%   % Compute sum of 2 images
%		Y = MrImage();
%		Z = MrImage();
%		X = Y.plus(Z);
%
%   % OR (cool overload!):
%       X = Y+Z
%
%   See also MrImage perform_binary_operation

% Author:   Saskia Bollmann & Lars Kasper
% Created:  2014-11-13
% Copyright (C) 2014 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public Licence (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

outputImage = this.perform_binary_operation(otherImage, @plus);


function []= sounding_powerlaw_sens(hydro,wet)
% function that computes the power law coefficents and performs a
% sensitivity analysis.
%
%     Copyright (C) 2015  Bekaert David - University of Leeds
%     Email: eedpsb@leeds.ac.uk or davidbekaert.com
%
%     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 2 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License along
%     with this program; if not, write to the Free Software Foundation, Inc.,
%     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
%
% By David Bekaert -- University of Leeds 2013
% modifications
% 05/2013   DB:     Compute the power law coefficients for dedicated days.
% 08/2013   DB:     Fix the case when a netdelay cannot be computed.
% 02/2014   DB:     Add non-stamps file support of ifgs-based estimation.
% 03/2014   DB:     Fix the computation of the sensitivity analysis in
%                   absence of soundign for specific SAR dates.
% 08/2014   DB:     Expand for PS network and SB network for SAR date estimation
% 03/2016   DB:     remove warning on ifgday for non-stamps processing

if nargin<1
    hydro=1;
    wet=1;
end

% getting the data from the parms_aps file
n_months = getparm_aps('sounding_months');
sounding_start_date = getparm_aps('sounding_start_date');
sounding_end_date = getparm_aps('sounding_end_date');
sounding_dir=getparm_aps('sounding_dir');
time_stamp = getparm_aps('sounding_time_stamp');
sounding_ifg_dates = getparm_aps('sounding_ifg_dates');
time_stamp_str = [];
for k=1:size(time_stamp,1)
    if k>1
        time_stamp_str = [time_stamp_str '_' time_stamp(k,:)];
    else
        time_stamp_str = [time_stamp(k,:)];
    end
end

% checking if the directory exists
if exist([sounding_dir filesep],'dir')~=7
        error('myApp:argChk', ['The specified filepath of the sounding data does not exist,...  \nAbort,... \n'])
end
% current directry
curdir = pwd;

% getting the interferogram data information
% loading the data
stamps_processed = getparm_aps('stamps_processed');
if strcmp(stamps_processed,'y')
   fprintf('Stamps processed structure \n')
   ll_matfile = getparm_aps('ll_matfile');
   ps = load(ll_matfile);
   dates = ps.day;
   lonlat = ps.lonlat;

    % getting the parms file list from stamps to see the final ifg list

    % constructing the matrix with master and slave dates
    if strcmp(getparm('small_baseline_flag'),'y')
        % for SB
        ifgs_ix = ps.ifgday_ix;

    else
        n_ifg = ps.n_ifg;
        % slightly different for PS.
        date_slave_ix = [1:n_ifg]';
        % the master dates
        date_master_ix = repmat(ps.master_ix,size(date_slave_ix,1),1);
        % ix interferograms
        ifgs_ix = [date_master_ix date_slave_ix];
    end
else
    % getting the dates in jullian format
    ifgday_matfile = getparm_aps('ifgday_matfile');
    ifgs_dates = load(ifgday_matfile);
    ifgs_dates = ifgs_dates.ifgday;
    dates = reshape(ifgs_dates,[],1);
    dates = unique(dates);
    dates = datenum(num2str(dates),'yyyymmdd');
    dates = sort(dates);        % dates increasing with time

    % getting the ix position for the master and slave dates with respect
    % to the times
    date_master = datenum(num2str(ifgs_dates(:,1)),'yyyymmdd');
    date_slave = datenum(num2str(ifgs_dates(:,2)),'yyyymmdd');

    for k=1:size(date_master,1)
        [date_master_ix(k,1)] = find(date_master(k,1)==dates);
        [date_slave_ix(k,1)] = find(date_slave(k,1)==dates);
    end

    % ix interferograms
    ifgs_ix = [date_master_ix date_slave_ix];
    clear date_master_ix date_slave_ix
end

if strcmp(sounding_ifg_dates,'y')
    % estimate for interferogram dates
    stamps_processed = getparm_aps('stamps_processed');
    if strcmp(stamps_processed,'y')
        ps = load(ll_matfile);
        date_start_vector = datestr(ps.day-15,'yyyymmdd');
        date_end_vector = datestr(ps.day+15,'yyyymmdd');

    else

        ifgday_matfile = getparm_aps('ifgday_matfile');
        ifgs_dates = load(ifgday_matfile);
        ifgs_dates = ifgs_dates.ifgday;
        dates = reshape(ifgs_dates,[],1);
        dates = unique(dates);
        dates = datenum(num2str(dates),'yyyymmdd');
        dates = sort(dates);        % dates increasing with time

        date_start_vector =  datestr(dates-15,'yyyymmdd');
        date_end_vector =  datestr(dates+15,'yyyymmdd');
    end

    % save name of the output data
    if hydro==1 && wet==0
        save_name = ['Powerlaw_sensitivity_hydro_SAR_dates_1month_' time_stamp_str 'Hr.mat'  ];
    elseif hydro==0 && wet==1
        save_name = ['Powerlaw_sensitivity_wet_SAR_dates_1month_' time_stamp_str 'Hr.mat'  ];
    else
        save_name = ['Powerlaw_sensitivity_SAR_dates_1month_' time_stamp_str 'Hr.mat'  ];
    end
    if exist([sounding_dir filesep 'Powerlaw'],'dir')~=7
        mkdir([sounding_dir filesep 'Powerlaw']);
    end

else
    % estimate is on fixed intervals
    % in case no start or end time is given, get it from the data files
    if isempty(sounding_start_date) || isempty(sounding_end_date)

        if exist('sounding.list','file')~=2
            % making a list of all the sounding files
            [dummy dummy2] = system('echo sounding_list > sounding.list');
            clear dummy dummy2
            for k=1:size(time_stamp,1)
                command_str = ['ls [0-9]???????_' time_stamp(k,:) '.mat >> sounding.list'];
                [dummy dummy2] = system(command_str);
                clear dummy dummy2
            end
        end
        temp = tdfread('sounding.list');
        [dummy dummy2] = system('rm sounding.list');
        clear dummy dummy2
        date_list_temp = temp.sounding_list(:,[1:8]);

        % selecting a date range when requested
        clear ix

        if isempty(start_date)
            sounding_start_date = date_list_temp(1,:);
        end
        if isempty(end_date)
            sounding_end_date = date_list_temp(end,:);
        end
        clear date_list_temp
    end

    % putting the variables in the right set-up
    start_year = str2num(sounding_start_date(1:4));
    end_year = str2num(sounding_end_date(1:4));
    start_str = sounding_start_date(5:6);
    end_str = sounding_end_date(5:6);
    start_month = str2num(start_str);
    end_month = str2num(end_str);

    % save name of the output data
   if hydro==1 && wet==0
        save_name = ['Powerlaw_sensitivity_hydro_' num2str(n_months) 'month_' time_stamp_str 'Hr_' num2str(start_year) start_str '_' num2str(end_year) end_str '.mat'  ];
    elseif hydro==0 && wet==1
        save_name = ['Powerlaw_sensitivity_wet_' num2str(n_months) 'month_' time_stamp_str 'Hr_' num2str(start_year) start_str '_' num2str(end_year) end_str '.mat'  ];
   else
        save_name = ['Powerlaw_sensitivity_' num2str(n_months) 'month_' time_stamp_str 'Hr_' num2str(start_year) start_str '_' num2str(end_year) end_str '.mat'  ];
    end
    if exist([sounding_dir filesep 'Powerlaw'],'dir')~=7
        mkdir([sounding_dir filesep 'Powerlaw']);
    end

    if exist([sounding_dir filesep 'Powerlaw'],'dir')~=7
        mkdir([sounding_dir filesep 'Powerlaw']);
    end

    % generating the periods
    month_str = ['01';'02';'03';'04';'05';'06';'07';'08';'09';'10';'11';'12'];
    counter = 1;
    % runnign the computation of the powerlaw in batches
    for k=1:end_year-start_year+1
        if k==end_year-start_year+1 & k>1
            for l=1:end_month
                date_start_temp = datenum([num2str(start_year+k-1)  month_str(l,:) '01'],'yyyymmdd');
                date_start_month_vector(counter,:) = datestr(date_start_temp,'yyyymmdd');
                counter = counter+1;
            end
        elseif k==end_year-start_year+1 & k==1
            for l=start_month:end_month
                date_start_temp = datenum([num2str(start_year+k-1)  month_str(l,:) '01'],'yyyymmdd');
                date_start_month_vector(counter,:) = datestr(date_start_temp,'yyyymmdd');
                counter = counter+1;
            end
        else
            for l=1:12
                date_start_temp = datenum([num2str(start_year+k-1)  month_str(l,:) '01'],'yyyymmdd');
                date_start_month_vector(counter,:) = datestr(date_start_temp,'yyyymmdd');
                counter = counter+1;
            end
        end
    end

    n_months_total = size(date_start_month_vector,1);
    for k=1:ceil(n_months_total/n_months)
        k_lower = n_months*(k-1)+1;
        k_upper = n_months*(k)+1;

        if k_upper>n_months_total
            k_upper = n_months_total;
        end
        date_start_vector(k,:)=date_start_month_vector(k_lower,:);
        date_end_vector(k,:) = datestr(datenum(date_start_month_vector(k_upper,:),'yyyymmdd')-1,'yyyymmdd');
    end

end

% Remove those months outside the users request
if ~isempty(sounding_start_date)
    ix_drop = find(datenum(date_start_vector,'yyyymmdd')<datenum(sounding_start_date,'yyyymmdd'));
else
    ix_drop = [];
end
if ~isempty(sounding_end_date)
    ix_drop = [ix_drop ; find(datenum(date_end_vector,'yyyymmdd')>datenum(sounding_end_date,'yyyymmdd'))];
end
ix_drop = unique(ix_drop);
date_end_vector(ix_drop,:)=[];
date_start_vector(ix_drop,:)=[];

% Computing the power law coefficients
for k=1:size(date_start_vector,1)
    fprintf(['\n' num2str(k) '/' num2str(size(date_start_vector,1)) ' completed \n']);
	% When having sounding data estimate the powerlaw and b coefficients

    [alpha_all,alpha_hc,h_0_threshold,n_soundings] = sounding(date_start_vector(k,:),date_end_vector(k,:),[],hydro,wet);
	alpha_vector_all(k,1) = alpha_all;
    alpha_vector(k,1) = alpha_hc;
	n_soundings_vector(k,1)=n_soundings;
	h0_vector(k,1)=h_0_threshold;

end
save([sounding_dir filesep 'Powerlaw' filesep save_name],'alpha_vector_all','alpha_vector','n_soundings_vector','h0_vector','date_start_vector','date_end_vector')

cd(curdir)
if strcmp(sounding_ifg_dates,'y')
    % estimate for interferogram dates
    stamps_processed = getparm_aps('stamps_processed');
    if strcmp(stamps_processed,'y')
        fprintf('Updating powerlaw parameters with new values. \n')

        % removing NaN by replacing them with the other SAR date estimates
        ix_alpha_fix = find(isnan(alpha_vector(:,1)));
        if ~isempty(ix_alpha_fix)
           if length(ix_alpha_fix)~=length(alpha_vector)
                fprintf([num2str(length(ix_alpha_fix)) ' out of ' num2str(length(alpha_vector)) ' SAR dates where set to the mean alpha due to lack of sounding data. \n'])
                alpha_vector(isnan(alpha_vector))=nanmean(alpha_vector);
                alpha_vector_fix = alpha_vector;
                save([sounding_dir filesep 'Powerlaw' filesep save_name],'-append','alpha_vector_fix','ix_alpha_fix')

           else
              fprintf('None of the SAR dates where estimate using sounding data \n')
           end

        end

        ix_height_fix = find(isnan(h0_vector(:,1)));
        if ~isempty(ix_height_fix)
           if length(ix_height_fix)~=length(h0_vector)
                fprintf([num2str(length(ix_height_fix)) ' out of ' num2str(length(alpha_vector)) ' SAR dates where set to the mean h0 due to lack of sounding data. \n'])
                h0_vector(isnan(h0_vector))=nanmean(h0_vector);
                h0_vector_fix = h0_vector;
                save([sounding_dir filesep 'Powerlaw' filesep save_name],'-append','h0_vector_fix','ix_height_fix')
           else
              fprintf('None of the SAR dates where estimate using sounding data \n')
           end
        end

        alpha_SAR = [alpha_vector(ifgs_ix(:,1)) alpha_vector(ifgs_ix(:,2))];
        h0_SAR = [h0_vector(ifgs_ix(:,1)) h0_vector(ifgs_ix(:,2))];

        % computing the mean between two SAR dates
        h0_InSAR = mean(h0_SAR,2);
        alpha_InSAR = mean(alpha_SAR,2);

        setparm_aps('powerlaw_h0',h0_InSAR');
        setparm_aps('powerlaw_alpha',alpha_InSAR');
    end
else

    % computing the mean between two SAR dates
    ix = isnan(h0_vector);
    powerlaw_h0 = mean(h0_vector(~ix));
    powerlaw_alpha = mean(alpha_vector(~ix));

    setparm_aps('powerlaw_h0',powerlaw_h0');
    setparm_aps('powerlaw_alpha',powerlaw_alpha');
end