function title = p12_title ( option )

%*****************************************************************************80
%
%% P12_TITLE sets the title for problem 12.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Output, string TITLE, the title of the problem.
%
  if ( option == 1 )
    title = 'Materially nonlinear problem, NPOLYS = 2, NDERIV = 1.';
  elseif ( option == 2 )
    title = 'Materially nonlinear problem, NPOLYS = 4, NDERIV = 1.';
  elseif ( option == 3 )
    title = 'Materially nonlinear problem, NPOLYS = 4, NDERIV = 2.';
  elseif ( option == 4 )
    title = 'Materially nonlinear problem, NPOLYS = 6, NDERIV = 1.';
  elseif ( option == 5 )
    title = 'Materially nonlinear problem, NPOLYS = 6, NDERIV = 2.';
  elseif ( option == 6 )
    title = 'Materially nonlinear problem, NPOLYS = 6, NDERIV = 3.';
  end

  return
end


function c = bitshift(a,b)
	checkinputs(a,b);

	if numel(b)==1
		if b<0
			c = uint64rightshift(uint64(a),uint64(-b));
		else
			c = uint64leftshift(uint64(a),uint64(b));
		end
	else
		c = zeros(size(b),'uint64');
		neg = b < 0;
		if numel(a)==1
			c(neg)  = uint64rightshift(uint64(a),uint64(-b(neg)));
			c(~neg) = uint64leftshift(uint64(a),uint64(b(~neg)));
		else
			c(neg)  = uint64rightshift(uint64(a(neg)),uint64(-b(neg)));
			c(~neg) = uint64leftshift(uint64(a(~neg)),uint64(b(~neg)));
		end
	end
end


function H = getHankelMatrix(xdat,stackmax)

H = zeros(stackmax,size(xdat,1)-stackmax);
for k=1:stackmax
    H(k,:) = xdat(k:end-stackmax-1+k,1);
end

end


function transparency_change(o2, multval)
% Change the transparency of blobs in an fmridisplay object
%
% :Inputs:
%
%   **multval:**
%        multiply transparency values by this.
%
% values < 1 makes blobs more transparent, > 1 makes blobs more opaque

for m = 1:length(o2.activation_maps)

    h = o2.activation_maps{m}.blobhandles;

    for i = 1:length(h)

        d1 = get(h(i), 'AlphaData');

        d1 = d1 .* multval;

        d1(d1 > 1) = 1;

        set(h(i), 'AlphaData', d1);

    end

end

end


function tnet = vl_simplenn_tidy(net)
%VL_SIMPLENN_TIDY  Fix an incomplete or outdated SimpleNN network.
%   NET = VL_SIMPLENN_TIDY(NET) takes the NET object and upgrades
%   it to the current version of MatConvNet. This is necessary in
%   order to allow MatConvNet to evolve, while maintaining the NET
%   objects clean.
%
%   The function is also generally useful to fill in missing default
%   values in NET.
%
%   See also: VL_SIMPLENN().

% Copyright (C) 2014-15 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).

tnet = struct('layers', {{}}, 'meta', struct()) ;

% copy meta information in net.meta subfield
if isfield(net, 'classes')
  tnet.meta.classes = net.classes ;
end

if isfield(net, 'normalization')
  tnet.meta.normalization = net.normalization ;
end

if isfield(net, 'meta')
  tnet.meta = net.meta ;
end

% copy layers
for l = 1:numel(net.layers)
  defaults = {'precious', false};
  layer = net.layers{l} ;

  % check weights format
  switch layer.type
    case {'conv', 'convt', 'bnorm'}
      if ~isfield(layer, 'weights')
        layer.weights = {...
          layer.filters, ...
          layer.biases} ;
        layer = rmfield(layer, 'filters') ;
        layer = rmfield(layer, 'biases') ;
      end
  end

  % check that weights inlcude moments in batch normalization
  if strcmp(layer.type, 'bnorm')
    if numel(layer.weights) < 3
      layer.weights{3} = ....
        zeros(numel(layer.weights{1}),2,'single') ;
    end
  end

  % fill in missing values
  switch layer.type
    case {'conv', 'pool'}
      defaults = [ defaults {...
        'pad', 0, ...
        'stride', 1, ...
        'opts', {}}] ;

    case 'convt'
      defaults = [ defaults {...
        'crop', 0, ...
        'upsample', 1, ...
        'numGroups', 1, ...
        'opts', {}}] ;

    case {'pool'}
      defaults = [ defaults {...
        'method', 'max', ...
        'pad', 0, ...
        'stride', 1, ...
        'opts', {}}] ;

    case 'relu'
      defaults = [ defaults {...
        'leak', 0}] ;

    case 'dropout'
      defaults = [ defaults {...
        'rate', 0.5}] ;

    case {'normalize', 'lrn'}
      defaults = [ defaults {...
        'param', [5 1 0.0001/5 0.75]}] ;

    case {'pdist'}
      defaults = [ defaults {...
        'noRoot', false, ...
        'aggregate', false, ...
        'p', 2, ...
        'epsilon', 1e-3} ];
  end

  for i = 1:2:numel(defaults)
    if ~isfield(layer, defaults{i})
      layer.(defaults{i}) = defaults{i+1} ;
    end
  end

  % save back
  tnet.layers{l} = layer ;
end


function zero_rc_test ( )

%*****************************************************************************80
%
%% ZERO_RC_TEST tests the ZERO_RC library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2013
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ZERO_RC_TEST:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the ZERO_RC library.\n' );

  zero_rc_test01 ( );
  zero_rc_test02 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ZERO_RC_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function c = minus(a,b)
	checkinputs(a,b);
	c = int64minus(int64(a),int64(b));
end


function this = read_matrix_from_workspace(this, inputMatrix)
% Reads in matrix from workspace, updates dimInfo according to data
% dimensions
%
%   Y = MrDataNd()
%   Y.read_matrix_from_workspace()
%
% This is a method of class MrDataNd.
%
% IN
%
% OUT
%
% EXAMPLE
%   read_matrix_from_workspace
%
%   See also MrDataNd

% Author:   Saskia Bollmann & Lars Kasper
% Created:  2016-10-12
% Copyright (C) 2016 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 License (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/>.

% check whether valid dimInfo now

% TODO: update dimInfo, but keeping information that is unaltered by
% changing data dimensions...
% e.g. via dimInfo.merge
hasDimInfo = isa(this.dimInfo, 'MrDimInfo');

this.data = inputMatrix;

% remove singleton 2nd dimension kept by size command
nSamples = size(this.data);
if numel(nSamples) == 2
    nSamples(nSamples==1) = [];
end
resolutions = ones(1, numel(nSamples));

% set dimInfo or update according to actual number of samples
if ~hasDimInfo
    this.dimInfo = MrDimInfo('nSamples', nSamples, ...
        'resolutions', resolutions);
else
    if any(nSamples) % only update dimInfo, if any samples loaded
        if (numel(nSamples) ~= this.dimInfo.nDims)
            % only display the warning of an non-empty dimInfo (i.e. nDims
            % ~=0) has been given
            if (this.dimInfo.nDims ~=0)
                warning('Number of dimensions in dimInfo (%d) does not match dimensions in data (%d), resetting dimInfo', ...
                    this.dimInfo.nDims, numel(nSamples));
            end
            this.dimInfo = MrDimInfo('nSamples', nSamples, ...
                'resolutions', resolutions);
        elseif ~isequal(this.dimInfo.nSamples, nSamples)
            % if nSamples are correct already, leave it at that, otherwise:

            currentResolution = this.dimInfo.resolutions;

            isValidResolution = ~any(isnan(currentResolution)) || ...
                ~any(isinf(currentResolution)) && ...
                numel(currentResolution) == numel(nSamples);

            if isValidResolution
                % use update of nSamples to keep existing offset of samplingPoints
                this.dimInfo.nSamples = nSamples;
            else % update with default resolutions = 1
                this.dimInfo.set_dims(1:this.dimInfo.nDims, 'nSamples', ...
                    nSamples, 'resolutions', resolutions);
            end

        end
    end
end


function events = in_events_micromed(sFile, EventFile)
% IN_EVENTS_MICROMED: Open a Micromed .EVT file.
%
% OUTPUT:
%    - events(i): array of structures with following fields (one structure per event type)

% @=============================================================================
% 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: Francois Tadel, 2021

% Initialize returned value
events = [];
% Read XML file
sXml = in_xml(EventFile);
if ~isfield(sXml, 'EventFile') || ~isfield(sXml.EventFile, 'Events') || ~isfield(sXml.EventFile.Events, 'Event') || isempty(sXml.EventFile.Events.Event)
    return;
end

% Get recordings start date (timestamp)
acq = sFile.header.acquisition;
start_time = posixtime(datetime(acq.year, acq.month, acq.day, acq.hour, acq.min, acq.sec, 0));

% Parse structure
sEvents = sXml.EventFile.Events.Event;
Markers = cell(length(sEvents), 3);
for iMrk = 1:length(sEvents)
    % Get time from the beginning of the file
    Markers{iMrk,1} = [posixtime(datetime(sEvents(iMrk).Begin.text, 'InputFormat', 'yyyy-MM-dd''T''HH:mm:ss.SSS')) - start_time; ...
                       posixtime(datetime(sEvents(iMrk).End.text, 'InputFormat', 'yyyy-MM-dd''T''HH:mm:ss.SSS')) - start_time];
    % TEXT: Interpret as event ename if the string is short
    if (length(sEvents(iMrk).Text.text) < 50)
        Markers{iMrk,2} = sEvents(iMrk).Text.text;
        if isfield(sEvents(iMrk), 'ExtraText') && isfield(sEvents(iMrk).ExtraText, 'text') && ~isempty(sEvents(iMrk).ExtraText.text)
            Markers{iMrk,3} = sEvents(iMrk).ExtraText.text;
        end
    % Otherwise, consider it as a note for an event EVT
    else
        Markers{iMrk,2} = 'EVT';
        Markers{iMrk,3} = sEvents(iMrk).Text.text;
    end
end

% List of events
uniqueEvt = unique(Markers(:,2)');
% Initialize returned structure
events = repmat(db_template('event'), 0);
% Create events list
for iEvt = 1:length(uniqueEvt)
    % Find all the occurrences of event #iEvt
    iOcc = find(strcmpi(Markers(:,2)', uniqueEvt{iEvt}));
    % Compute samples from the file start
    samples = round([Markers{iOcc,1}] .* sFile.prop.sfreq);
    notes = Markers(iOcc,3)';
    % Detect if simple events (duration <= 2 samples)
    if ~any(samples(2,:) - samples(1,:) > 2)
        samples(2,:) = [];
    end
    % Adjust if there are segments in the file
    if isfield(sFile.header, 'segment') && ~isempty(sFile.header.segment)
        for iOcc = 1:size(samples,2)
            iSeg = find(samples(1,iOcc) >= [sFile.header.segment.time], 1, 'last');
            if ~isempty(iSeg)
                samples(:,iOcc) = ...
                    samples(:,iOcc) - sFile.header.segment(iSeg).time ...                    % Offset from beginning of segment (in samples)
                    + sFile.header.segment(iSeg).sample - sFile.header.segment(1).sample ... % Offset from beginning of file
                    + round(sFile.prop.times(1) .* sFile.prop.sfreq);                        % Start of the file
            end
        end
    end
    % Detect events outside of the file definition
    iOut = find((samples(end,:) < sFile.prop.times(1) .* sFile.prop.sfreq) | (samples(1,:) > sFile.prop.times(2) .* sFile.prop.sfreq));
    if ~isempty(iOut)
        disp([sprintf('BST> Event "%s": %d occurrences outside of the recordings [%1.3fs,%1.3fs]:   ', uniqueEvt{iEvt}, length(iOut), sFile.prop.times(1), sFile.prop.times(2)), ...
              sprintf('%1.3fs ', samples(1,iOut) ./ sFile.prop.sfreq)]);
        samples(:,iOut) = [];
        notes(iOut) = [];
    end
    if isempty(samples)
        continue;
    end
    % Add event structure
    iNew = length(events) + 1;
    events(iNew).label      = uniqueEvt{iEvt};
    events(iNew).epochs     = ones(1, size(samples,2));
    events(iNew).times      = samples ./ sFile.prop.sfreq;
    events(iNew).reactTimes = [];
    events(iNew).select     = 1;
    events(iNew).channels   = [];
    events(iNew).notes      = notes;
end


function [tests,pass,perf]=test_rdmat(varargin)

inputs={'verbose'};
verbose=0;
for n=1:nargin
    if(~isempty(varargin{n}))
        eval([inputs{n} '=varargin{n};'])
    end
end

%Test the examples
test1_str=['wfdb2mat(''mitdb/200'');' ...
           '[tm,signal,Fs,siginfo]=rdmat(''200m'');'...
           '[signal2,Fs2,tm2]=rdsamp(''200m'');' ...
           'if(sum(abs(signal-signal2)) ~=0);error(''data reading failed'');end' ...
           ];

test_string={test1_str};
clean_up={['delete([pwd filesep ''mitdb'' filesep ''*'']);' ...
          'if(exist([pwd filesep ''mitdb''],''dir''));rmdir([pwd filesep ''mitdb''],''s'');end;' ...
          'delete([pwd filesep ''200m*'']);' ...
          ]};
[tests,pass,perf]=test_wrapper(test_string,clean_up,verbose);


function geneID = findGeneIDs(model, geneList)
% Finds gene numbers in a model
%
% USAGE:
%
%    geneID = findGeneIDs(model, geneList)
%
% INPUTS:
%    model:       COBRA model structure
%    geneList:    List of genes
%
% OUTPUT:
%    geneID:      List of gene IDs corresponding to `geneList`
%
% .. Author: - Jeff Orth 7/16/09

if (iscell(geneList))
    [tmp,geneID] = ismember(geneList,model.genes);
else
    geneID = find(strcmp(model.genes,geneList));
    if (isempty(geneID))
        geneID = 0;
    end
    if (length(geneID) > 1)
        geneID = geneID(1);
    end
end


function stla_write ( output_file_name, node_num, face_num, node_xyz, ...
  face_node, face_normal )

%*****************************************************************************80
%
%% STLA_WRITE writes graphics information to an ASCII StereoLithography file.
%
%  Example:
%
%    solid MYSOLID
%      facet normal 0.4 0.4 0.2
%        outerloop
%          vertex  1.0 2.1 3.2
%          vertex  2.1 3.7 4.5
%          vertex  3.1 4.5 6.7
%        end loop
%      end facet
%      ...
%      facet normal 0.2 0.2 0.4
%        outerloop
%          vertex  2.0 2.3 3.4
%          vertex  3.1 3.2 6.5
%          vertex  4.1 5.5 9.0
%        end loop
%      end facet
%    end solid MYSOLID
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 September 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    3D Systems, Inc,
%    Stereolithography Interface Specification,
%    October 1989.
%
%  Parameters:
%
%    Input, string OUTPUT_FILE_NAME, the name of the output file.
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer FACE_NUM, the number of faces.
%
%    Input, real NODE_XYZ(3,NODE_NUM), the node coordinates.
%
%    Input, integer FACE_NODE(3,FACE_NUM), the nodes making faces.
%
%    Input, integer FACE_MAX, the maximum number of faces.
%
%    Input, real FACE_NORMAL(3,FACE_NUM), the normal vector at each face.
%
  text_num = 0;
%
%  Open the file.
%
  iunit = fopen ( output_file_name, 'wt' );

  if ( iunit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'STLA_WRITE - Fatal error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', output_file_name );
    error ( 'STLA_WRITE - Fatal error!' );
  end

  fprintf ( iunit, 'solid MYSOLID\n' );
  text_num = text_num + 1;

  for face = 1 : face_num

    fprintf ( iunit, '  facet normal  %14f  %14f  %14f\n', ...
      face_normal(1:3,face) );
    text_num = text_num + 1;

    fprintf ( iunit, '    outer loop\n' );
    text_num = text_num + 1;

    for vertex = 1 : 3

      node = face_node(vertex,face);

      fprintf ( iunit, '      vertex    %14f  %14f  %14f\n', ...
        node_xyz(1:3,node) );
      text_num = text_num + 1;

    end

    fprintf ( iunit, '    end loop\n' );
    text_num = text_num + 1;
    fprintf ( iunit, '  end facet\n' );
    text_num = text_num + 1;

  end

  fprintf ( iunit, 'end solid MYSOLID\n' );
  text_num = text_num + 1;

  fclose ( iunit );

  return
end


function stats = extractStats(net)
% -------------------------------------------------------------------------
sel = find(cellfun(@(x) isa(x,'dagnn.Loss'), {net.layers.block})) ;
stats = struct() ;
for i = 1:numel(sel)
    stats.(net.layers(sel(i)).outputs{1}) = net.layers(sel(i)).block.average ;
end


function imData=read_file(path_to_file,sframe,num2read,options,im_info)

% Reads uncompressed multipage .tiff, .hdf5, .avi or .raw files
% Usage:  my_data=read_file('path_to_data_file, start frame, num to read);

% INPUTS:
% path_to_file:     location of file to be read
% sframe:           first frame to read (optional, default: 1)
% num2read:         number of frames to read (optional, default: read the whole file)
% options:          options for reading .raw or .bin files
% im_info:          information about the file (if already present)

% OUTPUT:
% imData:           data in array format

% Written by Eftychios A. Pnevmatikakis, Simons Foundation

if nargin<2 || isempty(sframe); sframe = 1; end
if nargin<3 || isempty(num2read); num2read = Inf; end

[~,~,ext] = fileparts(path_to_file);

if strcmpi(ext,'.tiff') || strcmpi(ext,'.tif') || strcmpi(ext,'.btf')
    if ~exist('im_info','var')
        im_info = imfinfo(path_to_file);
    end
    TifLink = Tiff(path_to_file, 'r');
    num2read = min(num2read,length(im_info)-sframe+1);
    imData = zeros(im_info(1).Height,im_info(1).Width,num2read,'like',TifLink.read());
    for i=1:num2read
       TifLink.setDirectory(i+sframe-1);
       imData(:,:,i)=TifLink.read();
    end
    TifLink.close()
    %imData = loadtiff(path_to_file,sframe,num2read);
elseif strcmpi(ext,'.hdf5') || strcmpi(ext,'.h5')
%     info = hdf5info(path_to_file);
%     dims = info.GroupHierarchy.Datasets.Dims;
%     name = info.GroupHierarchy.Datasets.Name;
    info = h5info(path_to_file);
    dims = info.Datasets.Dataspace.Size;
    name = info.Datasets.Name;
    if nargin < 3
        num2read = dims(end)-sframe+1;
    end
    num2read = min(num2read,dims(end)-sframe+1);
%    imData = h5read(path_to_file,name,[ones(1,length(dims)-1),sframe],[dims(1:end-1),num2read]);
    imData = h5read(path_to_file,['/',name],[ones(1,length(dims)-1),sframe],[dims(1:end-1),num2read]);
elseif strcmpi(ext,'.avi')
    v = VideoReader(path_to_file);
    if v.Duration*v.FrameRate < sframe
        imData = [];
    else
        if nargin < 3
            num2read = v.Duration*v.FrameRate-sframe+1;
        end
        num2read = min(num2read,v.Duration*v.FrameRate-sframe+1);
        Y1 = readFrame(v);
        imData = zeros(v.Height,v.Width,num2read,'like',Y1);
        i = 1;
        if sframe == 1
            imData(:,:,i-sframe+1) = Y1;
        end
        while hasFrame(v)  && (i - sframe + 1 < num2read)
            video = readFrame(v);
            i = i + 1;
            if i >= sframe
                imData(:,:,i-sframe+1) = video;
            end
            if i - sframe + 1 >= num2read
                break;
            end
        end
    end
elseif strcmpi(ext,'.raw')
    if nargin < 4 || ~isfield(options,'d1') || isempty(options.d1);
        options.d1 = input('What is the total number of rows? \n');
    end
    if nargin < 4 || ~isfield(options,'d2') || isempty(options.d2);
        options.d2 = input('What is the total number of columns? \n');
    end
    if nargin < 4 || ~isfield(options,'bitsize') || isempty(options.bitsize);
        options.bitsize = 2;
    end
    imData = read_raw_file(path_to_file,sframe,num2read,[options.d1,options.d2],options.bitsize);
else
    error('Unknown file extension. Only .tiff, .avi and .hdf5 files are currently supported');
end


%% PRIME_POOL uses the MATLABPOOL command to run the PRIME code.
%
%  Discussion:
%
%    Output printed by the function appears directly on the screen.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 March 2010
%
%  Author:
%
%    John Burkardt
%
  n = 10000;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'PRIME_POOL\n' );
  fprintf ( 1, '  Count prime numbers from 1 to %d\n', n );

  matlabpool open local 4

  total = prime_fun ( n );

  matlabpool close

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Total is %d\n', total );


% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open 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.
%
% dslam_open 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 dslam_open. If not, see <http://www.gnu.org/licenses/>.

function plot3DSubGraph(graph, values, linespec, colorspec, markerspec)
%PLOTGRAPH Plots the Pose3's in a values, with optional covariances
%   Finds all the Pose2 objects in the given Values object and plots the graph.
% If a Marginals object is given, this function will also plot marginal
% covariance ellipses for each pose.

import gtsam.*

if ~exist('linespec', 'var') || isempty(linespec)
    linespec = '-';
end

if ~exist('colorspec', 'var') || isempty(colorspec)
    colorspec = 'r';
end

if ~exist('markerspec', 'var') || isempty(markerspec)
    markerspec = '';
end

holdstate = ishold;
hold on

L = {linespec};
% Draw graph
for i = 0:graph.size - 1
    factor = graph.at(i);
    if size(factor.keys) == 2
        key1 = factor.keys.at(0);
        key2 = factor.keys.at(1);

        try
        pose1 = values.atPose3(key1);
        pose2 = values.atPose3(key2);
        % draw a line - do not show loop closures if showLoopClosures is
        % false
        key1_sym = symbolChr(key1);
        key2_sym = symbolChr(key2);
        key1_index = symbolIndex(key1);
        key2_index = symbolIndex(key2);

        if key1_sym ~= key2_sym
            plot([pose1.x, pose2.x], [pose1.z, pose2.z], 'Linestyle', '-', 'Color', 'k', 'Marker', markerspec, 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'r', 'alpha', 0.5)
        else
            plot([pose1.x, pose2.x], [pose1.z, pose2.z], 'Linestyle', '-o', 'Color', colorspec)
        end

        catch
        end
    end
end

if ~holdstate
    hold off
end

end


function dtiExportFibers(handles,varargin)
%
%  dtiExportFibers(handles,options);
%
% Exports non-mrDiffusion fiber/path file formats. Currently supports:
%
% 1. MetroTrac (*.pdb) format
%
% Options may include:
% * 'current' | 'selected' | 'all': save only the current fiber group, allow
% user to select some groups to save, or save all the fibers (default is
% current)
%
%
% HISTORY:
% 2007.06.15 AJS: wrote it.
%

fn = handles.defaultPath;
if(exist(fullfile(fn, 'fibers'),'dir')); fn = fullfile(fn, 'fibers', filesep); end
if isempty(handles.fiberGroups), warndlg('No fiber groups'); return; end

% Parse options
if(~isempty(varargin))
    if(~isempty(strmatch('all', lower(varargin)))); saveMode = 3;
    elseif(~isempty(strmatch('sel', lower(varargin)))); saveMode = 2;
    else saveMode = 1;
    end
    normFlag = ~isempty(strmatch('norm', lower(varargin)));
end

if(normFlag)
    if(length(handles.t1NormParams)>1)
        [id,OK] = selector(1:length(handles.t1NormParams), ...
        {handles.t1NormParams.name}, 'Select the coord space...');
        if(~OK); disp('user canceled.'); return; end
    else
        id = 1;
    end
    if(~isfield(handles.t1NormParams(id), 'name'))
        % in the old days, we didn't name the coord space.
        warning('no name associated with this coord space; using MNI.'); %#ok<WNTAG>
        handles.t1NormParams(id).name = 'MNI';
    end
    %coordSpace = handles.t1NormParams(id).name;
    if(isfield(handles.t1NormParams(id),'sn'))
         xform = handles.t1NormParams(id);
        if(~isfield(xform,'deformX')||isempty(xform.deformX))
        	disp('Computing inverse deformation...');
            [xform.deformX, xform.deformY, xform.deformZ] = mrAnatInvertSn(xform.sn);
            handles.t1NormParams(id).deformX = xform.deformX;
            handles.t1NormParams(id).deformY = xform.deformY;
            handles.t1NormParams(id).deformZ = xform.deformZ;
        end
        % xform that goes from acpc space to the deformation field space.
        xform.inMat = inv(xform.sn.VF.mat);
    else
        disp(['There is no xform specified- saving untransformed coords as "' coordSpace '".']);
        xform = [];
    end
else
    %coordSpace = 'acpc';
    xform = [];
end

% Save 'em.
if(saveMode==1)
    fg = handles.fiberGroups(handles.curFiberGroup);
    fn = fullfile(fn, [strrep(fg.name, ' ', '_') '.pdb']);
    [f, p] = uiputfile({'*.pdb';'*.*'}, ['Export Fibers "' fg.name '"...'], fn);
    if(isnumeric(f)), disp('Export current fibers canceled.'); return; end
    % For pdb format lets save out in ACPC space
    %mtrExportFibers(fg, fullfile(p, f), handles.xformToAcpc, xform);
    % Assume fiber groups are in AcPc space
    mtrExportFibers(fg, fullfile(p, f));
    disp('Current fibers exported.');
elseif(saveMode==2 || saveMode==3)
    p = uigetdir(fn, 'Fiber Group directory');
    if isnumeric(p), disp('Export FGs ... canceled.'), return; end
    if(saveMode==2)
        sList = dtiSelectFGs(handles,'Export FGs');
        if isempty(sList), disp('Export Fiber Groups ... canceled.'); return; end
    else
        sList = 1:length(handles.fiberGroups);
    end
    FGs = handles.fiberGroups(sList);
    for ii=1:length(FGs)
        fg = FGs(ii);
        fName = [strrep(fullfile(p,fg.name), ' ', '_') '.pdb'];
        mtrExportFibers(fg, fName, handles.xformToAcpc, xform);
    end
    disp(sprintf('Saved %.0f Fiber Groups.\n', length(FGs)))
end

return;


function bSimilar = isSimilarImage(img1, img2)
%
%
%       bSimilar = isSimilarImage(img1, img2)
%
%
%       This image checks if two images are the same.
%
%     Copyright (C) 2018  Francesco Banterle
%
%     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/>.
%

[r1, c1, ~] = size(img1);
[r2, c2, ~] = size(img2);

bSimilar = (r1 == r2) & (c1 == c2);

end


function disp(t,name)
%DISP Command window display of a ttensor.
%
%   DISP(T) displays a ttensor with no name.
%
%   DISP(T,NAME) display a ttensor with the given name.
%
%   See also TTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2010, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by Brett Bader and Tamara Kolda.
% http://csmr.ca.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2010) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in tensor_toolbox/LICENSE.txt
% $Id: disp.m,v 1.14 2010/03/19 23:46:31 tgkolda Exp $

if ~exist('name','var')
    name = 'ans';
end

fprintf(1,'%s is a ttensor of size %s\n', name, tt_size2str(size(t)));
disp(t.core, sprintf('\t%s.core',name));

for j = 1 : ndims(t)
    fprintf('\t%s.U{%d} = \n', name, j);
    output = tt_matrix2cellstr(t.u{j});
    fprintf('\t\t%s\n',output{:});
end


function wfa = nan2wfa(nan)
%
%EVENT_NAN2WFA: Convert events from NAN form to WFA form.
%      WFA - WaveForm Array form, events separated in nx1 waveform array
%          - (or) into 1xl cell array, each cell containing a nx1 waveform
%          - array of events from a separate station/channel.
%      NAN - NaN form, (1xl waveform) for l station/channels with events in
%            one time series w/ non-events = NaN.
%      SST - Start/Stop Times form, nx2 array of matlab start/stop times
%            (or) 1xl cell array, each cell containing nx2 start/stop
%            times from a separate station/channel.
%
%USAGE: events = nan2wfa(nan)
%
%INPUTS: nan - Waveform object of events (1x1) in NAN form
%
%OUTPUTS: wfa - Waveform object array of events (WFA form)
%
% See also NAN2SST, SSD2SST, SST2NAN, SST2SSD, SST2VAL, SST2WFA,
%          WFA2NAN, WFA2SST
%
% Author: Dane Ketner, Alaska Volcano Observatory
% $Date$
% $Revision$

for l = 1:numel(nan)
   dat = get(nan(l),'data');
   l_d = length(dat);
   wfa{l} = [];

   if isnan(dat(1))
      inevent = 0;
   elseif ~isnan(dat(1))
      indx1 = 1;
      inevent = 1;
   end

   for n = 2:l_d-1
      if (inevent == 0) && ~(isnan(dat(n)))
         indx1 = n;
         inevent = 1;
      elseif (inevent == 1) && (isnan(dat(n)))
         indx2 = n-1;
         wfa{l}=[wfa{l}; extract(nan(l),'INDEX',indx1,indx2)];
         inevent = 0;
      end
   end

   if ~isnan(dat(l_d)) && (inevent == 1)
      indx2 = l_d;
      wfa{l}=[wfa{l}; extract(nan(1),'INDEX',indx1,indx2)];
   end
end

if numel(wfa) == 1
   wfa = wfa{1};
end


function varargout = expression( nm, varargin )

%EXPRESSION Declares a single CVX object for storing subexpressions.
%   EXPRESSION x
%   where x is a valid MATLAB variable nm, declares a scalar expression
%   holder for the current cvx model. Like a variable, an expression holder
%   can be used in constraints and objectives, according to the DCP ruleset.
%   However, unlike a variable, an expression holder is initialized to
%   zero, because the intent is for it to hold intermediate computations.
%
%   EXPRESSION x(n1,n2,...,nk)
%   declares a vector, matrix, or array expression holder with dimensions
%   n1, n2, ..., nk, each of which must be nonnegative integers. The value
%   of the expression holder is initialized to zero.
%
%   Examples:
%      variable x y
%      expression z
%      z = 2 * x - y;
%
%   See also EXPRESSIONS.

global cvx___
prob = evalin( 'caller', 'cvx_problem', '[]' );
if ~isa( prob, 'cvxprob' ),
    cvx_throw( 'No CVX model exists in this scope.' );
elseif isempty( cvx___.problems ) || cvx___.problems( end ).self ~= prob,
    cvx_throw( 'Internal CVX data corruption. Please CLEAR ALL and rebuild your model.' );
elseif nargin > 1,
    cvx_throw( 'Too many input arguments.\nTrying to declare multiple expression holders? Use the EXPRESSIONS keyword instead.', 1 ); %#ok
end

%
% Step 1: separate the name from the parenthetical, verify the name
%

toks = regexp( nm, '^\s*([a-zA-Z]\w*)\s*(\(.*\))?\s*$', 'tokens' );
if isempty( toks ),
    cvx_throw( 'Invalid variable specification: %s', nm );
end
toks = toks{1};
x.name = toks{1};
x.size = toks{2};
if x.name(end) == '_',
    cvx_throw( 'Invalid expression specification: %s\n   Names ending in underscores are reserved for internal use.', nm );
end

%
% Step 2: Parse the size. In effect, all we do here is surround what is
% replace the surrounding parentheses with square braces and evaluate. All
% that matters is the result is a valid size vector. In particular, it
% need to be a simple comma-delimited list.
%

if isempty( x.size ),
	x.size = [1,1];
else
    try
        x.size = evalin( 'caller', [ '[', x.size(2:end-1), '];' ] );
    catch exc
        throw( exc );
    end
    x.size = cvx_get_dimlist( x.size );
end

%
% Step 3. Initialize
%

v = cvx( x.size, [] );
if nargout > 0,
    varargout{1} = v;
else
    assignin( 'caller', x.name, v );
end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [out1,out2,out3] = ginput2(arg1)
%GINPUT Graphical input from mouse.
%   [X,Y] = GINPUT(N) gets N points from the current axes and returns
%   the X- and Y-coordinates in length N vectors X and Y.  The cursor
%   can be positioned using a mouse (or by using the Arrow Keys on some
%   systems).  Data points are entered by pressing a mouse button
%   or any key on the keyboard except carriage return, which terminates
%   the input before N points are entered.
%
%   [X,Y] = GINPUT gathers an unlimited number of points until the
%   return key is pressed.
%
%   [X,Y,BUTTON] = GINPUT(N) returns a third result, BUTTON, that
%   contains a vector of integers specifying which mouse button was
%   used (1,2,3 from left) or ASCII numbers if a key on the keyboard
%   was used.

%   Copyright (c) 1984-96 by The MathWorks, Inc.
%   $Revision: 5.18 $  $Date: 1996/11/10 17:48:08 $

% Fixed version by Jean-Yves Bouguet to have a cross instead of 2 lines
% More visible for images

out1 = []; out2 = []; out3 = []; y = [];
c = computer;
if ~strcmp(c(1:2),'PC') & ~strcmp(c(1:2),'MA')
    tp = get(0,'TerminalProtocol');
else
    tp = 'micro';
end

if ~strcmp(tp,'none') & ~strcmp(tp,'x') & ~strcmp(tp,'micro'),
    if nargout == 1,
        if nargin == 1,
            eval('out1 = trmginput(arg1);');
        else
            eval('out1 = trmginput;');
        end
    elseif nargout == 2 | nargout == 0,
        if nargin == 1,
            eval('[out1,out2] = trmginput(arg1);');
        else
            eval('[out1,out2] = trmginput;');
        end
        if  nargout == 0
            out1 = [ out1 out2 ];
        end
    elseif nargout == 3,
        if nargin == 1,
            eval('[out1,out2,out3] = trmginput(arg1);');
        else
            eval('[out1,out2,out3] = trmginput;');
        end
    end
else

    fig = gcf;
    figure(gcf);

    if nargin == 0
        how_many = -1;
        b = [];
    else
        how_many = arg1;
        b = [];
        if  isstr(how_many) ...
            | size(how_many,1) ~= 1 | size(how_many,2) ~= 1 ...
            | ~(fix(how_many) == how_many) ...
            | how_many < 0
            error('Requires a positive integer.')
        end
        if how_many == 0
            ptr_fig = 0;
            while(ptr_fig ~= fig)
                ptr_fig = get(0,'PointerWindow');
            end
            scrn_pt = get(0,'PointerLocation');
            loc = get(fig,'Position');
            pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)];
            out1 = pt(1); y = pt(2);
        elseif how_many < 0
            error('Argument must be a positive integer.')
        end
    end

    pointer = get(gcf,'pointer');
    set(gcf,'pointer','crosshair');
    fig_units = get(fig,'units');
    char = 0;

    while how_many ~= 0
        % Use no-side effect WAITFORBUTTONPRESS
        waserr = 0;
        eval('keydown = wfbp;', 'waserr = 1;');
        if(waserr == 1)
      if(ishandle(fig))
        set(fig,'pointer',pointer,'units',fig_units);
        error('Interrupted');
      else
        error('Interrupted by figure deletion');
      end
        end

        ptr_fig = get(0,'CurrentFigure');
        if(ptr_fig == fig)
            if keydown
                char = get(fig, 'CurrentCharacter');
                button = abs(get(fig, 'CurrentCharacter'));
                scrn_pt = get(0, 'PointerLocation');
                set(fig,'units','pixels')
                loc = get(fig, 'Position');
                pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)];
                set(fig,'CurrentPoint',pt);
            else
                button = get(fig, 'SelectionType');
                if strcmp(button,'open')
                    button = 1; %b(length(b));
                elseif strcmp(button,'normal')
                    button = 1;
                elseif strcmp(button,'extend')
                    button = 2;
                elseif strcmp(button,'alt')
                    button = 3;
                else
                    error('Invalid mouse selection.')
                end
            end
            pt = get(gca, 'CurrentPoint');

            how_many = how_many - 1;

            if(char == 13) % & how_many ~= 0)
                % if the return key was pressed, char will == 13,
                % and that's our signal to break out of here whether
                % or not we have collected all the requested data
                % points.
                % If this was an early breakout, don't include
                % the <Return> key info in the return arrays.
                % We will no longer count it if it's the last input.
                break;
            end

            out1 = [out1;pt(1,1)];
            y = [y;pt(1,2)];
            b = [b;button];
        end
    end

    set(fig,'pointer',pointer,'units',fig_units);

    if nargout > 1
        out2 = y;
        if nargout > 2
            out3 = b;
        end
    else
        out1 = [out1 y];
    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function key = wfbp
%WFBP   Replacement for WAITFORBUTTONPRESS that has no side effects.

% Remove figure button functions
fprops = {'windowbuttonupfcn','buttondownfcn', ...
          'windowbuttondownfcn','windowbuttonmotionfcn'};
fig = gcf;
fvals = get(fig,fprops);
set(fig,fprops,{'','','',''})

% Remove all other buttondown functions
ax = findobj(fig,'type','axes');
if isempty(ax)
    ch = {};
else
    ch = get(ax,{'Children'});
end
for i=1:length(ch),
    ch{i} = ch{i}(:)';
end
h = [ax(:)',ch{:}];
vals = get(h,{'buttondownfcn'});
mt = repmat({''},size(vals));
set(h,{'buttondownfcn'},mt);

% Now wait for that buttonpress, and check for error conditions
waserr = 0;
eval(['if nargout==0,', ...
      '   waitforbuttonpress,', ...
      'else,', ...
      '   keydown = waitforbuttonpress;',...
      'end' ], 'waserr = 1;');

% Put everything back
if(ishandle(fig))
  set(fig,fprops,fvals)
  set(h,{'buttondownfcn'},vals)
end

if(waserr == 1)
  error('Interrupted');
end

if nargout>0, key = keydown; end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% EEG_DECODECHAN - given an input EEG dataset structure, output a new EEG data structure
%                retaining and/or excluding specified time/latency, data point, channel,
%                and/or epoch range(s).
% Usage:
%   >> [chaninds chanlist] = eeg_decodechan(chanlocs, chanlist);
%
% Inputs:
%   chanlocs  - channel location structure
%   chanlist  - list of channels, numerical indices [1 2 3 ...] or string
%               'cz pz fz' or cell array { 'cz' 'pz' 'fz' }. Can also be
%               a list of channel types (see below)
%   field     - ['labels'|'type'] channel field to match. Default is
%               'labels'
%   ignoremissing - [true|false] ignore channel in channel list that would
%               be missing in the dataset.
%
% Outputs:
%   chaninds  - integer array with the list of channel indices
%   chanlist  - cell array with a list of channel labels
%
% Author: Arnaud Delorme, SCCN/INC/UCSD, 2009-
%
% see also: EEGLAB

% Copyright (C) 2001 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [ chaninds, chanlist ] = eeg_decodechan(chanlocs, chanstr, field, ignoremissing)

if nargin < 2
    help eeg_decodechan;
    return;
end
if nargin < 3
    field = 'labels';
end
if nargin < 4
    ignoremissing = false;
end
if isstruct(chanlocs) && isfield(chanlocs, 'chanlocs')
    chanlocs = chanlocs.chanlocs;
end

if isempty(chanlocs) && ischar(chanstr)
    chaninds = str2num(chanstr);
    chanlist = chaninds;
    return;
end

if ischar(chanstr)
    % convert chanstr
    % ---------------
    chanstr(find(chanstr == ']')) = [];
    chanstr(find(chanstr == '[')) = [];
    chanlistnum = [];
    chanstr  = [ ' ' chanstr ' ' ];
    try
        chanlist = eval( [ '{' chanstr '}' ] );
        chanlistnum = cellfun(@str2double, chanlist);
        if isnumeric(chanlist{1}) chanlist = [ chanlist{:} ]; end
    catch
        chanlist = {};
        sp = find(chanstr == ' ');
        for i = 1:length(sp)-1
            c = chanstr(sp(i)+1:sp(i+1)-1);
            if ~isempty(c)
                chanlist{end+1} = c;
                if isnan(str2double(chanlocs(1).(field))) % channel labels are not numerical
                    if ~isnan(str2double(c))
                        chanlistnum(end+1) = str2double(c);
                    end
                end
            end
        end
        if length(chanlistnum) == length(chanlist)
            chanlist = chanlistnum;
        end
    end
else
    chanlist = chanstr;
end

% convert to values
% -----------------
chanval = 0;
if isnumeric(chanlist)
    chanval = chanlist;
end
% chanval  = [];
% if iscell(chanlist)
%     for ind = 1:length(chanlist)
%
%         valtmp = str2double(chanlist{ind});
%         if ~isnan(valtmp)
%              chanval(end+1) = valtmp;
%         else chanval(end+1) = 0;
%         end
%     end
% else
%     chanval = chanlist;
% end

% convert to numerical
% --------------------
if all(chanval) > 0
    chaninds = chanval;
    chanlist = chanval;
else
    chaninds = [];
    alllabs  = lower({ chanlocs.(field) });
    chanlist = lower(chanlist);
    for ind = 1:length(chanlist)
        indmatch = find(strcmp(alllabs,chanlist{ind})); %#ok<STCI>
        if ~isempty(indmatch)
            for tmpi = 1:length(indmatch)
                chaninds(end+1) = indmatch(tmpi);
            end
        else
            try
                eval([ 'chantmp = ' chanlist{ind} ';' ]);
                if isempty(chantmp)
                     error([ 'Channel ''' chanlist{ind} ''' not found' ]);
                else
                    if chantmp <= length(chanlist)
                        chaninds(end+1) = chantmp;
                    elseif ~ignoremissing
                        error([ 'Channel ''' chanlist{ind} ''' not found' ]);
                    end
                end
            catch
                if ~ignoremissing
                    error([ 'Channel ''' chanlist{ind} ''' not found' ]);
                end
            end
        end
    end
end
chaninds = sort(chaninds);
if ~isempty(chanlocs)
    if any(chaninds > length(chanlocs)) || any(chaninds <= 0)
        error('Channel index out of range')
    end
    chanlist = { chanlocs(chaninds).(field) };
else
    chanlist = {};
end


% emacs, this is -*-Matlab-*- mode
%
% readradfile    reads the BlueC *.rad files
%
% *.rad files contain paprameters of the radial distortion
% [K,kc] = readradfile(name)
% name ... name of the *.rad file with its full path
%
% K ... 3x3 calibration matrix
% kc ... 4x1 vector of distortion parameters
%
% $Id: readradfile.m,v 2.0 2003/06/19 12:07:16 svoboda Exp $
function [K,kc] = readradfile(name);

fid = fopen(name,'r');
if fid<0
  error(sprintf('Could not open %s. Missing rad files?',name'))
end

for i=1:3,
  for j=1:3,
	buff = fgetl(fid);
        str_end = buff(7:end);
        if str_end(end)==';'
          str_end = str_end(1:end-1);
        end
        K(i,j) = str2num(str_end);
  end
end

buff = fgetl(fid);
for i=1:4,
  buff = fgetl(fid);
  str_end = buff(7:end);
  if str_end(end)==';'
    str_end = str_end(1:end-1);
  end
  kc(i) = str2num(str_end);
end

fclose(fid);

return;


function display(SO3F,varargin)
% standard output

if check_option(varargin,'skipHeader')
  disp('  bingham component');
else
  displayClass(SO3F,inputname(1),[],'moreInfo',symChar(SO3F),varargin{:});
  disp(' ');
end

if SO3F.antipodal, disp('  antipodal: true'); end

disp(['  kappa: ',xnum2str(SO3F.kappa)]);
disp(['  weight: ' xnum2str(mean(SO3F))])
disp(' ');

end


function annotation = addDetector2LM(boundingBox, boxScores, boxClass, annotation)
%
% annotation = addDetector2LM(boundingBox, boxScores, boxClass, annotation)
%
% Input
%    boundingBox: matrix [Nobjects x 4]. Each row is boundingBox(i,:) = [xmin xmax ymin ymax]
%    boxScores  : vector [Nobjects]
%    boxClass   : string with the object name. I assume all bounding boxes
%                 belong to a single class
%    annotation : the object will be added to the annotation struct
%                 (optional)
%
% Output
%    annotation

nDetections = size(boundingBox, 1);

% check if there are other objects
nObjects = 0;

if nargin == 4
    if isfield(annotation, 'object')
        nObjects = length(annotation.object);
    end
else
    annotation = [];
end

% Translate bounding boxes into LabelMe format:
if nDetections>0
    k = nObjects;
    for i = 1:nDetections;%1:nDetections
        k = k+1;
        annotation.object(k).name = boxClass;
        annotation.object(k).confidence = boxScores(i);
        %X = [boundingBox(i,1) boundingBox(i,2) boundingBox(i,2) boundingBox(i,1) boundingBox(i,1)];
        %Y = [boundingBox(i,3) boundingBox(i,3) boundingBox(i,4) boundingBox(i,4) boundingBox(i,3)];

        X = [boundingBox(i,1) boundingBox(i,2) boundingBox(i,2) boundingBox(i,1)];
        Y = [boundingBox(i,3) boundingBox(i,3) boundingBox(i,4) boundingBox(i,4)];

        annotation.object(k).polygon = setLMpolygon(X,Y);
        annotation.object(k).deleted = 0;
        annotation.object(k).date = '';
        annotation.object(k).id = i;
    end
end


%% IPOPT Install for OPTI Toolbox
% Copyright (C) 2012 Jonathan Currie (I2C2)

% This file will help you compile Interior Point OPTimizer (IPOPT) for use
% with MATLAB.

% My build platform:
% - Windows 8 x64
% - Visual Studio 2012
% - Intel Compiler XE (FORTRAN)
% - Intel Math Kernel Library

% NOTE - From OPTI v1.71 IPOPT is now dynamically linked against the
% MathWorks supplied libmwma57.dll (HSL MA57). This step is optional
% as we are also compiling MUMPS. Alternatively you can skip MUMPS,
% and just use MA57! Also be aware libmwma57.dll does not play well
% on unconstrained problems due in part to missing MeTiS, thus the
% ma57 pivot order option is overidden automatically.

% To recompile you will need to get / do the following:

% 1) Get IPOPT
% IPOPT is available from http://www.coin-or.org/download/source/Ipopt/.
% Download the latest version of the source.

% 2) Compile IPOPT
% The easiest way to compile IPOPT is to use the Visual Studio Project
% Builder included with OPTI. Use the following commands, substituting the
% required path on your computer:

% %% Visual Studio Builder Commands
% path = 'full path to IPOPT here'; %e.g. 'C:\Solvers\IPOPT'
% mumpspath = 'full path to MUMPS here'; %e.g. 'C:\Solvers\MUMPS'
% sdir = [path '\src'];
% hdrs = {[mumpspath '\include'], [mumpspath '\libseq'], [path '\..\BuildTools\headers']};
% name = 'libipopt';
% opts = [];
% opts.exPP = {'IPOPT_BUILD','FUNNY_MA57_FINT','COINHSL_HAS_MA57','_CRT_SECURE_NO_WARNINGS'};
% opts.exFolder = {'Apps','Algorithm\Inexact','contrib\LinearSolverLoader'};
% opts.exclude = 'AmplTNLP.cpp';
% VS_WriteProj(sdir,name,hdrs,opts)
% %%

% Once complete, you will have a directory called IPOPT\libipopt. Open the
% Visual Studio 2012 project file, then complete the following steps:
%   a) Open config_default.h and make the following changes:
%       - Add #define COIN_HAS_METIS 1 under the METIS library line
%       - Add #define COIN_HAS_MUMPS 1 under the MUMPS library line
%       - Comment the line #define COIN_HAS_HSL 1
%       - Comment the line #define COIN_HAS_ASL 1
%   b) Build a Win32 or x64 Release to compile the code.
%   c) Copy the generated .lib file to the following folder:
%
%   OPTI/Solvers/ipopt/Source/lib/win32 or win64
%
%   You will also need to copy all header files from IPOPT/src to
%   the following folder (inc sub-directories):
%
%   OPTI/Solvers/ipopt/Source/Include

% 3) Complete MUMPS Compilation
% Supplied with this toolbox is the MUMPS linear system solver and METIS
% libraries. These are required when linking IPOPT so make sure you have
% built these first! See opti_MUMPS_Install.m in Solvers/mumps for more
% information.

% 4) libmwma57 Compilation
% As MathWorks only supply a dll, I have manually created an import library
% for libmwma57.dll, so we can link against it. The project and source for
% this is included in libmwma57.zip. It is optional to recompile this, as I
% have included compiled libraries for you!

% 5) IPOPT MEX Interface
% Supplied with the IPOPT source is a MEX interface by Dr. Peter Carbonetto.
% This is in Ipopt/contrib/MatlabInterface/src. However I have modified
% parts of this interface, so you must use the version supplied with OPTI.

% 6) Compile the MEX File
% The code below will automatically include all required libraries and
% directories to build the IPOPT MEX file. Once you have completed all the
% above steps, simply run this file to compile IPOPT! You MUST BE in the
% base directory of OPTI!

clear ipopt

% Modify below function if it cannot find Intel MKL on your system.
[mkl_link,mkl_for_link] = opti_FindMKL();
% Get Arch Dependent Library Path
libdir = opti_GetLibPath();
% Dependency Paths
mumpsdir = '..\..\mumps\Source\';

fprintf('\n------------------------------------------------\n');
fprintf('IPOPT MEX FILE INSTALL\n\n');

%Get IPOPT Libraries
post = ' -IInclude -IInclude/Common -IInclude/BuildTools -IInclude/Interfaces -IInclude/LinAlg -IInclude/LinAlg/TMatrices -IInclude/Algorithm ';
post = [post ' -L' libdir ' -llibIpopt -llibmwma57 -DhaveMA57'];
%Get MUMPS Libraries
post = [post ' -I' mumpsdir '\Include -L' mumpsdir libdir '-llibdmumps_c -llibdmumps_f -llibseq_c -llibseq_f -llibmetis -llibpord'];
%Get Intel Fortran Libraries (for MUMPS build) & MKL Libraries (for BLAS)
post = [post mkl_link mkl_for_link];

%Common Args
post = [post ' -DIPOPT_BUILD -output ipopt'];

%CD to Source Directory
cdir = cd;
cd 'Solvers/ipopt/Source';

%Compile & Move
pre = 'mex -v -largeArrayDims matlabexception.cpp matlabfunctionhandle.cpp matlabjournal.cpp iterate.cpp ipoptoptions.cpp options.cpp sparsematrix.cpp callbackfunctions.cpp matlabinfo.cpp matlabprogram.cpp ipopt.cpp';
try
    eval([pre post])
    movefile(['ipopt.' mexext],'../','f')
    fprintf('Done!\n');
catch ME
    cd(cdir);
    error('opti:ipopt','Error Compiling IPOPT!\n%s',ME.message);
end
cd(cdir);
fprintf('------------------------------------------------\n');


function simple_xy_rd ( )

%*****************************************************************************80
%
%% SIMPLE_XY_RD reads a 2 dimensional array from a NETCDF file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 December 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Russ Rew, Glenn Davis, Steve Emmerson, Harvey Davies, Ed Hartne,
%    The NetCDF User's Guide,
%    Unidata Program Center, March 2009.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SIMPLE_XY_RD:\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read a small 2D array from a NETCDF file.\n' );
%
%  Open the file.
%
  nc_filename = 'simple_xy.nc';
  ncid = netcdf.open ( nc_filename, 'NOWRITE' );
%
%  Get the varid of the data based on its name.
%
  varid = netcdf.inqVarID ( ncid, 'data_out' );
%
%  Read the data.
%
  data_out = netcdf.getVar ( ncid, varid );
%
%  Compare the data to the expected value.
%
  nj = 6;
  ni = 12;

  for j = 1 : nj
    for i = 1 : ni
      if ( data_out(i,j) ~= ( j - 1 ) * ni - ( i - 1 ) )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'SIMPLE_XY_RD - Fatal error!\n' );
        fprintf ( 1, '  Entry (%d,%d) of array seems to be wrong.\n', i, j );
      end
    end
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The data is correct.\n' );
%
%  Close the file.
%
  netcdf.close ( ncid );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The data was read from the file "%s".\n', nc_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SIMPLE_XY_RD:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


%% Example of using KST class for interfacing with KUKA iiwa robots

% An example script, it is used to show how to use the different
% functions of the KUKA Sunrise Toolbox

% First start the server on the KUKA iiwa controller
% Then run the following script in Matlab

% This example works with Sunrise application version KST_1.7  and higher.
% Copyright Mohammad SAFEEA, 26th-June-2018

close all;clear;clc;
warning('off')
%% Create the robot object
ip='172.31.1.147'; % The IP of the controller
arg1=KST.LBR7R800; % choose the robot iiwa7R800 or iiwa14R820
arg2=KST.Medien_Flansch_elektrisch; % choose the type of flange
Tef_flange=eye(4); % transofrm matrix of EEF with respect to flange
iiwa=KST(ip,arg1,arg2,Tef_flange); % create the object

%% Start a connection with the server
flag=iiwa.net_establishConnection();
if flag==0
    return;
end
pause(1);
disp('Doing some stuff')

%% move to initial position
jPos={0,pi*20/180,0,-pi*70/180,0,pi*60/180,0}; % initial confuguration
relVel=0.15; % relative velocity
iiwa.movePTPJointSpace(jPos, relVel); % point to point motion in joint space

%% Linear relative motion of end effector, relative to base frame
deltaX=0;deltaY=0;deltaZ=100.;
Pos{1}=deltaX;
Pos{2}=deltaY;
Pos{3}=deltaZ;

vel=250; % linear velocity of end effector, mm/sec
iiwa.movePTPLineEefRelBase(Pos, vel);
Pos{3}=-deltaZ;
iiwa.movePTPLineEefRelBase(Pos, vel);

%% Linear relative motion of end effector, relative to EEF frame
Pos{1}=deltaX;
Pos{2}=deltaY;
Pos{3}=deltaZ;

vel=150; % linear velocity of end effector, mm/sec

iiwa.movePTPLineEefRelEef(Pos, vel);
Pos{3}=-deltaZ;
iiwa.movePTPLineEefRelEef(Pos, vel);

%% turn off the server
iiwa.net_turnOffServer();
warning('on');


function r = join(rs, joinall)
%JOIN Join similar structures of arrays to one structure of arrays
%
%   JOIN can be used to combine statistics collected from several
%   independent simulations which collect samples to structure
%   of arrays.
%
%   R = JOIN(RS) returns structure of arrays R, RS is array or
%   cell-array of similar structures of arrays. Arrays of R are
%   contanation of arrays in RS. Arrays having first dimension 1
%   are not concataned, but copied from the first element of RS.
%
%   R = JOIN(RS, JOINALL) if JOINALL is 1, arrays having first
%   dimension 1 are concataned.
%
%   See also
%     THIN

% Copyright (C) 2000-2006  Aki Vehtari
%
% This software is distributed under the GNU General Public
% Licence (version 3 or later); please refer to the file
% Licence.txt, included with the software, for details.

if nargin <2
  joinall=false;
end

[m,n]=size(rs);

r=[];
if (m==1 & n==1)
  r=rs;
else
  if iscell(rs)
    rs=[rs{:}];
  end
  if isstruct(rs)
    r=rs(1);
    names = fieldnames(r);
    for i1=1:size(names,1)
      switch names{i1}
       case 'rstate'
        continue
       case 'type'
        r.type=rs(1).(names{1});
        continue
      end
      tmp1=getfield(r,names{i1});
      if iscell(tmp1)
	%tmp2=eval(['cat(1,rs(:).' names{i1} ')']);
	tmp2=cat(1,rs(:).(names{i1}));
	for i2=1:length(tmp1(:))
	  %eval(['r.' names{i1} '{i2}=cat(1,tmp2{:,i2});']);
	  r.(names{i1}){i2}=cat(1,tmp2{:,i2});
	end
      elseif length(tmp1) > 1 || joinall
	%eval(['r.' names{i1} '=cat(1,rs.' names{i1} ');']);
	r.(names{i1})=cat(1,rs.(names{i1}));
      end
    end
  end
end


function [ height, ival, val ] = p09_gx ( option )

%*****************************************************************************80
%
%% P09_GX is used by problem 9.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Output, real HEIGHT, the height of the structure.
%
%    Output, integer IVAL, the index of the variable being fixed.
%
%    Output, real VAL, the value of the fixed variable.
%
  height = 2.0;
  ival = 4;

  if ( option == 1 | option == 5 | option == 9 )
    val = 0.00;
  elseif ( option == 2 | option == 6 | option == 10 )
    val = 0.25;
  elseif ( option == 3 | option == 7 | option == 11 )
    val = 0.50;
  elseif ( option == 4 | option == 8 | option == 12 )
    val = 1.00;
  elseif ( option == 13 )
    val = 0.00;
  end

  return
end


function val = get(f, propName)
%GET    GET method for SPHEREFUN class.
%   P = GET(F, PROPNAME) returns the property P specified in the string
%   PROPNAME from the SPHEREFUN object F. Valid entries for the string
%   PROPNAME are:
%    'DOMAIN'
%    'COLS'
%    'ROWS'
%    'PIVOTVALUES'
%    'PIVOTLOCATIONS'
%    'BLOCKDIAG'
%    'IDXPLUS'
%    'IDXMINUS'
%    'NONZEROPOLES'

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Loop through an array of SPHEREFUN objects.
if ( numel(f) > 1 )
    val = cell(numel(f));
    for k = 1:numel(f)
        val{k} = get(f(k), propName);
    end
    return
end

% Get the properties.
switch ( propName )
    case 'domain'
        val = f.domain;
    case 'cols'
        val = f.cols;
    case 'rows'
        val = f.rows;
    case 'pivotValues'
        val = f.pivotValues;
    case 'pivotLocations'
        val = f.pivotLocations;
    case 'blockDiag'
        val = f.blockDiag;
    case 'idxPlus'
        val = f.idxPlus;
    case 'idxMinus'
        val = f.idxMinus;
    case 'nonZeroPoles'
        val = f.nonZeroPoles;
    otherwise
        error('CHEBFUN:SPHEREFUN:get:propName', ...
            [propName,' is not a valid SPHEREFUN property.'])
end


function [] = anim8_DIC3DPP_faceMeasure_onImages_n_n(DIC3DPPresults,pairIndex,faceMeasureString,varargin)
%% function for plotting 3D-DIC post-processing results from step 4 projected on the 2D images
% plotting the images with the 3D face measure results plotted on top
% on the left side the images from the reference camera (reference image and current images), and on the right side the
% images from the deformed camera
%
% calling options:
% [] = anim8_DIC3D_faceMeasures_onImages_n_n(DIC3DPPresults,pairIndex,faceMeasureString);
% [] = anim8_DIC3D_faceMeasures_onImages_n_n(DIC3DPPresults,pairIndex,faceMeasureString,optStruct);
%
% INPUT:

%%
Points=DIC3DPPresults.DIC2Dinfo{pairIndex}.Points;
nCamRef=DIC3DPPresults.DIC2Dinfo{pairIndex}.nCamRef;
nCamDef=DIC3DPPresults.DIC2Dinfo{pairIndex}.nCamDef;
nImages=DIC3DPPresults.DIC2Dinfo{pairIndex}.nImages;
currentFacesLogic=DIC3DPPresults.FacePairInds==pairIndex;
currentPointIndex=find(DIC3DPPresults.PointPairInds==pairIndex);
firstCurrentPointIndex=currentPointIndex(1);
F=DIC3DPPresults.Faces(currentFacesLogic,:);
F=F-firstCurrentPointIndex+1;
FaceCorr=cell(nImages,1);
for ii=1:nImages
    FaceCorr{ii}=DIC3DPPresults.FaceCorrComb{ii}(currentFacesLogic,:);
end
ImSet=cell(2*nImages,1);
ImPaths=DIC3DPPresults.DIC2Dinfo{pairIndex}.ImPaths;

% if ImPaths are not valid (for example if using on another computer, ask
% user to provide a new folder where all the images are located.
try
    ImSet{1}=imread(ImPaths{1});
catch
    newPath=uigetdir([],'Path to images is invalid. Please provide the correct path to the images (the folder containing all camera folders with the processed gray images)');
    for ii=1:length(ImPaths)
        [a,b,c]=fileparts(ImPaths{ii});
        as = strsplit(a,{'\','/'});
        ImPaths{ii}=[newPath '\' as{end} '\' b c];
    end
end

for ii=1:2*nImages
    ImSet{ii}=imread(ImPaths{ii});
    if size(ImSet{ii},3)==3
        ImSet{ii}=rgb2gray(ImSet{ii});
    end
end

switch nargin
    case 3 % in case no results were entered
        optStruct=struct;
    case 4
        optStruct=varargin{1};
    otherwise
        error('wrong number of input arguments');
end

%% cut out point with large correlation coefficient
if ~isfield(optStruct,'CorCoeffCutOff')
    CorCoeffCutOff=max(max(cell2mat(FaceCorr)));
else
    CorCoeffCutOff=optStruct.CorCoeffCutOff;
end

for ii=1:nImages
    FaceCorr{ii}(FaceCorr{ii}>CorCoeffCutOff)=NaN;
end

%%
FC=cell(nImages,1);
switch faceMeasureString
    case {'J','Lamda1','Lamda2'}
        for ii=1:nImages
            FC{ii}=DIC3DPPresults.Deform.(faceMeasureString){ii}(currentFacesLogic,:);
        end
        if isfield(optStruct,'cMap')
            cMap=optStruct.cMap;
        else
            cMap=coldwarm;
        end
        if ~isfield(optStruct,'FClimits')
            FCmax=0;
            for ii=1:nImages
                FCmax=max([max(abs(FC{ii}(~isnan(FaceCorr{ii}))-1)) FCmax]);
            end
            FClimits=[1-FCmax 1+FCmax];
        else
            FClimits=optStruct.FClimits;
        end
    case {'Emgn','emgn','Eeq','eeq','EShearMax','eShearMax'}
        for ii=1:nImages
            FC{ii}=DIC3DPPresults.Deform.(faceMeasureString){ii}(currentFacesLogic,:);
        end
        if isfield(optStruct,'cMap')
            cMap=optStruct.cMap;
        else
            cMap='parula';
        end
        if ~isfield(optStruct,'FClimits')
            FCmax=0;
            for ii=1:nImages
                FCmax=max([max(FC{ii}(~isnan(FaceCorr{ii}))) FCmax]);
            end
            FClimits=[0 FCmax];
        else
            FClimits=optStruct.FClimits;
        end

    case {'Epc1','Epc2','epc1','epc2'}
        for ii=1:nImages
            FC{ii}=DIC3DPPresults.Deform.(faceMeasureString){ii}(currentFacesLogic,:);
        end
        if isfield(optStruct,'cMap')
            cMap=optStruct.cMap;
        else
            cMap=coldwarm;
        end
        if ~isfield(optStruct,'FClimits')
            FCmax=0;
            for ii=1:nImages
                FCmax=max([max(abs(FC{ii}(~isnan(FaceCorr{ii})))) FCmax]);
            end
            FClimits=[-FCmax FCmax];
        else
            FClimits=optStruct.FClimits;
        end
    otherwise
        error('unexpected face measure string. plots not created');
end

for ii=1:nImages
    FC{ii}(isnan(FaceCorr{ii}))=NaN;
end

%% cut out point with extreme face color values
if ~isfield(optStruct,'dataLimits')
    dataLimits=[min(min(cell2mat(FC))) max(max(cell2mat(FC)))];
else
    dataLimits=optStruct.dataLimits;
end

for ii=1:nImages
    FC{ii}(FC{ii}<dataLimits(1) | FC{ii}>dataLimits(2))=NaN;
end

%% plot
hf=cFigure;
hf.Units='normalized'; hf.OuterPosition=[.05 .05 .9 .9]; hf.Units='pixels';

% Ref
ii=1;
subplot(1,2,1)
hp1=imagesc(repmat(ImSet{ii},1,1,3)); hold on;
hp2=gpatch(F,Points{ii},FC{ii},'none',0.5);
pbaspect([size(ImSet{ii},2) size(ImSet{ii},1) 1])
hs1=title(['Ref (Cam ' num2str(nCamRef) ' frame ' num2str(1) ')']);
colormap(cMap);
hc1=colorbar;
caxis(FClimits)
title(hc1, faceMeasureString);
hc1.FontSize=16;
axis off

% Cur
ii=nImages+1;
subplot(1,2,2)
hp3=imagesc(repmat(ImSet{ii},1,1,3)); hold on
hp4=gpatch(F,Points{ii},FC{1},'none',0.5);
pbaspect([size(ImSet{ii},2) size(ImSet{ii},1) 1])
hs2=title(['Cur ' num2str(ii) ' (Cam ' num2str(nCamDef) ' frame ' num2str(1) ')']);
colormap(cMap);
hc2=colorbar;
caxis(FClimits);
title(hc2, faceMeasureString);
hc2.FontSize=16;
axis off

drawnow

%Create the time vector
animStruct.Time=linspace(0,1,nImages);

for ii=1:nImages
    Pnow1=Points{ii};
    Pnow2=Points{ii+nImages};

    cNow1=FC{ii};
    cNow2=FC{ii};

    TitleNow1=['Cur ' num2str(ii) ' (Cam ' num2str(nCamRef) ' frame ' num2str(ii) ')'];
    TitleNow2=['Cur ' num2str(ii) ' (Cam ' num2str(nCamDef) ' frame ' num2str(ii) ')'];

    %Set entries in animation structure
    animStruct.Handles{ii}=[hp1,hp3,hp2,hp2,hp4,hp4,hs1,hs2]; %Handles of objects to animate
    animStruct.Props{ii}={'CData','CData','Vertices','CData','Vertices','CData','String','String'}; %Properties of objects to animate
    animStruct.Set{ii}={repmat(ImSet{ii},1,1,3),repmat(ImSet{ii+nImages},1,1,3),Pnow1,cNow1,Pnow2,cNow2,TitleNow1,TitleNow2}; %Property values for to set in order to animate

end

anim8(hf,animStruct);
addFigureButtons;
end

%%
% MultiDIC: a MATLAB Toolbox for Multi-View 3D Digital Image Correlation
%
% License: <https://github.com/MultiDIC/MultiDIC/blob/master/LICENSE.txt>
%
% Copyright (C) 2018  Dana Solav
%
% If you use the toolbox/function for your research, please cite our paper:
% <https://engrxiv.org/fv47e>


function p = identityLine(ax,varargin)
%Draw an identity line on the current axis
%
%   p = identityLine(ax,varargin)
%
% Example:
%   plot(1:10,randn(1,10),'o')
%   identityLine(gca);
%   identityLine(gca,'linecolor',[1 0 0],'linestyle',':');
%
% (c) Stanford VISTA Team

%% if notDefined('ax'), ax = gca; end
p = inputParser;

% First optional argument
p.addRequired('ax',@isgraphics);

% Line color and style
vFunc = @(x) isnumeric(x) && logical(length(x) == 3);
p.addParameter('linecolor',[.5 .5 .5],vFunc);
p.addParameter('linestyle','--',@ischar);

% Parse inputs
p.parse(ax,varargin{:});
ax = p.Results.ax;
linecolor = p.Results.linecolor;
linestyle = p.Results.linestyle;

%% Set up axes
% Minimum and maximum of axes
xlim = get(ax,'xlim');
ylim = get(ax,'ylim');
mn = min(xlim(1),ylim(1));
mx = max(xlim(2),ylim(2));

% Here's the line
p = line([mn mx],[mn mx],'color',linecolor,'linestyle',linestyle);

% Set line properties.  These probably want to come in as an argument
set(p,'linewidth',2);
grid on

return


function Write_Kinematics(FileWritingSetup)
% Write_Kinematics(FileWritingSetup)
%
% FUNCTION:
%
%   This function takes the symbolic expressions for system energy,
%   position, and velocity, and uses them to write a function file.
%
%
% INPUTS: FileWritingSetup.
%
%   States = [2N x 2] cell array where the first column is the name of
%       the states, and the second column is comments about that states.
%       The index of a state in this array will match the index in the
%       dynamics function to be written. This function assumes a second
%       order system that is being written in first order form, so if
%       States{i,1} = z, then States{i+N,1} = dz; Furthermore, the output
%       (dStates) will have the form: dStates(i,:) = dz,
%       and dStates(i+N,:) = ddz.
%
%   Parameters = [K x 2] cell array with all of the input parameters. The
%       first column is for names, the second for comments
%
%   Kinematics = A struct with fields for the symbolic expressions for
%       position and velocity.
%
%   Directory = the name of the directory to write the files in.
%
%
% OUTPUTS:
%   This function will write the function kinematics()
%
%
% NOTE:
%
%   1)  It is assumed that the symbols in all of the inputs are self
%       consistent; that is - if Contacts{1,1} = H1, then there must be a field
%       in Dyn.(FileNames{i,1}) called H1.
%
%
% Written by Matthew Kelly
% December 6, 2013
% Cornell University
%
% See also DERIVE_EOM

States = FileWritingSetup.States;
Kinematics = FileWritingSetup.Kinematics;
Parameters = FileWritingSetup.Parameters;
Directory = FileWritingSetup.Directory;
Energy = FileWritingSetup.Energy;

r0 = Kinematics.r0;
r1 = Kinematics.r1;
r2 = Kinematics.r2;
dr0 = Kinematics.dr0;
dr1 = Kinematics.dr1;
dr2 = Kinematics.dr2;

CurrDir = cd;
if ~exist(Directory,'dir')
    mkdir(Directory);
end
cd(Directory);

fid = fopen('kinematics.m','w');

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                         Function Header                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

fprintf(fid,['function [Position, Velocity, Energy] = ' ...
    'kinematics(States, Parameters)\n']);
fprintf(fid,['%% function [Position, Velocity, Energy] = ' ...
    'kinematics(States, Parameters)\n']);
fprintf(fid,'%%\n%% Computer Generated File -- DO NOT EDIT \n');
fprintf(fid,['%%\n%% This function was created by the function '...
    'Write_Kinematics()\n']);
fprintf(fid,['%% ' datestr(now) '\n%%\n']);
fprintf(fid,'%%  RETURNS: \n');
fprintf(fid,'%%   Energy = a struct with fields: \n');
fprintf(fid,'%%       Potential \n');
fprintf(fid,'%%       Kinetic \n');
fprintf(fid,'%%       Total \n');
fprintf(fid,'%% \n');
fprintf(fid,'%%   Position = a struct with fields: \n');
fprintf(fid,'%%       footOne = [2xN] matrix with the position vectors for Foot One \n');
fprintf(fid,'%%       footTwo = [2xN] matrix with the position vectors for Foot Two \n');
fprintf(fid,'%%       hip = [2xN] matrix with the position vectors for the hip \n');
fprintf(fid,'%%       CoM = [2xN] matrix with the position vectors for the center of mass\n');
fprintf(fid,'%% \n');
fprintf(fid,'%%   Velocity = a struct with fields: \n');
fprintf(fid,'%%       footOne = [2xN] matrix with the velocity vectors for Foot One \n');
fprintf(fid,'%%       footTwo = [2xN] matrix with the velocity vectors for Foot Two \n');
fprintf(fid,'%%       hip = [2xN] matrix with the velocity vectors for the hip \n');
fprintf(fid,'%%       CoM = [2xN] matrix with the velocity vectors for the center of mass \n');
fprintf(fid,'%%\n');
fprintf(fid,'%%\n');
fprintf(fid,'%% Matthew Kelly \n%% Cornell University \n%% \n\n');
fprintf(fid,'%% \n');
fprintf(fid,'%% See also DERIVE_EOM \n');

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                           Parse Inputs                              %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

for idxState=1:size(States,1)
    fprintf(fid,[States{idxState,1} ' = States(' num2str(idxState) ',:); %% ' ...
        States{idxState,2} '\n']);
end %idxState
fprintf(fid,'\n');

for idxParam=1:size(Parameters,1)
    fprintf(fid,[Parameters{idxParam,1} ' = Parameters.' ...
        Parameters{idxParam,1} '; %% ' ...
        Parameters{idxParam,2} '\n']);
end %idxState
fprintf(fid,'\n');

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                          Write Outputs                              %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

fprintf(fid,'N = size(States,2);\n');

fprintf(fid,'\n');
fprintf(fid,'Position.footOne = zeros(2,N);\n');
fprintf(fid,'Position.footTwo = zeros(2,N);\n');
fprintf(fid,'Position.hip = zeros(2,N);\n');
fprintf(fid,['Position.footOne(1,:) = ' vectorize(r0(1)) ';\n']);
fprintf(fid,['Position.footOne(2,:) = ' vectorize(r0(2)) ';\n']);
fprintf(fid,['Position.footTwo(1,:) = ' vectorize(r2(1)) ';\n']);
fprintf(fid,['Position.footTwo(2,:) = ' vectorize(r2(2)) ';\n']);
fprintf(fid,['Position.hip(1,:) = ' vectorize(r1(1)) ';\n']);
fprintf(fid,['Position.hip(2,:) = ' vectorize(r1(2)) ';\n']);
fprintf(fid,'Position.CoM = (m1*Position.footOne + m2*Position.footTwo + M*Position.hip)/(m1+m2+M);\n');
fprintf(fid,'\n');

fprintf(fid,'if nargout > 1');
fprintf(fid,'\n');
fprintf(fid,'    Velocity.footOne = zeros(2,N);\n');
fprintf(fid,'    Velocity.footTwo = zeros(2,N);\n');
fprintf(fid,'    Velocity.hip = zeros(2,N);\n');
fprintf(fid,['    Velocity.footOne(1,:) = ' vectorize(dr0(1)) ';\n']);
fprintf(fid,['    Velocity.footOne(2,:) = ' vectorize(dr0(2)) ';\n']);
fprintf(fid,['    Velocity.footTwo(1,:) = ' vectorize(dr2(1)) ';\n']);
fprintf(fid,['    Velocity.footTwo(2,:) = ' vectorize(dr2(2)) ';\n']);
fprintf(fid,['    Velocity.hip(1,:) = ' vectorize(dr1(1)) ';\n']);
fprintf(fid,['    Velocity.hip(2,:) = ' vectorize(dr1(2)) ';\n']);
fprintf(fid,'    Velocity.CoM = (m1*Velocity.footOne + m2*Velocity.footTwo + M*Velocity.hip)/(m1+m2+M);\n');
fprintf(fid,'\n');
fprintf(fid,'end\n');

fprintf(fid,'if nargout > 2');
fprintf(fid,'\n');
fprintf(fid,['    Energy.Potential.m1 = ' ...
    vectorize(Energy.Potential.m1) ';\n']);
fprintf(fid,['    Energy.Potential.m2 = ' ...
    vectorize(    Energy.Potential.m2) ';\n']);
fprintf(fid,['    Energy.Potential.M = ' ...
    vectorize(Energy.Potential.M) ';\n']);
fprintf(fid,'    Energy.Potential.Total = Energy.Potential.m1 + Energy.Potential.m2 + Energy.Potential.M;\n');
fprintf(fid,'\n');
fprintf(fid,['    Energy.Kinetic.m1 = ' ...
    vectorize(Energy.Kinetic.m1) ';\n']);
fprintf(fid,['    Energy.Kinetic.m2 = ' ...
    vectorize(Energy.Kinetic.m2) ';\n']);
fprintf(fid,['    Energy.Kinetic.M = ' ...
    vectorize(Energy.Kinetic.M) ';\n']);
fprintf(fid,'    Energy.Kinetic.Total = Energy.Kinetic.m1 + Energy.Kinetic.m2 + Energy.Kinetic.M;\n');
fprintf(fid,'\n');
fprintf(fid,'    Energy.Total = Energy.Kinetic.Total + Energy.Potential.Total;\n');
fprintf(fid,'\n');
fprintf(fid,'end\n');
fprintf(fid,'end\n');
fclose(fid);

cd(CurrDir);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%  ~~~~~~~~~~~~~~~~~~~ Sub Functions  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function characterExpression = vectorize(symbolicExpression)

        characterExpression = char(symbolicExpression);
        characterExpression = strrep(characterExpression,'*','.*');
        characterExpression = strrep(characterExpression,'/','./');
        characterExpression = strrep(characterExpression,'^','.^');
end


function out = read_nervus_data(nrvHdr, segment, range, chIdx)
% read_nervus_data  Returns data from Nicolet file.
%
%   OUT = read_nervus_data(NRVHDR, SEGMENT, RANGE, CHIDX) returns data in an n x m array of
%   doubles where n is the number of datapoints and m is the number
%   of channels.
%
%   NRVHDR is a header from the function read_nervus_header
%   SEGMENT is the segment number in the file to read from
%   RANGE is a 1x2 array with the [StartIndex EndIndex]  - default: all
%   and CHIDX is a vector of channel indeces - default: all
%
%   FILENAME is the file name of a file in the Natus/Nicolet/Nervus(TM)
%   format (originally designed by Taugagreining HF in Iceland)
%
%   Based on ieeg-portal/Nicolet-Reader
%   at https://github.com/ieeg-portal/Nicolet-Reader
%
% Copyright (C) 2016, Jan Brogger and Joost Wagenaar
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%  FieldTrip is free software: you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation, either version 3 of the License, or
%  (at your option) any later version.
%
%  FieldTrip is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id: $

if nargin == 0
  ft_error('Missing argument');
elseif nargin == 1
  segment = 1;
  range = [1 nrvHdr.targetSampleCount];
  chIdx = nrvHdr.matchingChannels(:);
elseif nargin == 2
  range = [1 nrvHdr.targetSampleCount];
  chIdx = nrvHdr.matchingChannels(:);
elseif nargin == 3
  chIdx = nrvHdr.matchingChannels(:);
end

assert(length(range) == 2, 'Range is [firstIndex lastIndex]');
assert(range(1) > 0, 'Range is must start at one or higher');
assert(length(segment) == 1, 'Segment must be single value.');

% Get cumulative sum segments.
cSumSegments = [0 cumsum([nrvHdr.Segments.duration])];

% Reopen .e file.
h = fopen_or_error(nrvHdr.filename,'r','ieee-le');

% Find sectionID for channels
lChIdx = length(chIdx);
sectionIdx = zeros(lChIdx,1);
for i = 1:lChIdx
  tmp = find(strcmp(num2str(chIdx(i)-1),{nrvHdr.StaticPackets.tag}),1);
  sectionIdx(i) = nrvHdr.StaticPackets(tmp).index;
end

% Iterate over all requested channels and populate array.
out = zeros(range(2) - range(1) + 1, lChIdx);
for i = 1 : lChIdx
  %disp(['Reading channel ' num2str(chIdx) ' segment ' num2str(segment)]);
  % Get sampling rate for current channel
  curSF = nrvHdr.Segments(segment).samplingRate(chIdx(i));
  mult = nrvHdr.Segments(segment).scale(chIdx(i));

  % Find all sections
  allSectionIdx = nrvHdr.allIndexIDs == sectionIdx(i);
  allSections = find(allSectionIdx);

  % Find relevant sections
  sectionLengths = [nrvHdr.MainIndex(allSections).sectionL]./2;
  cSectionLengths = [0 cumsum(sectionLengths)];

  skipValues = cSumSegments(segment) * curSF;
  firstSectionForSegment = find(cSectionLengths > skipValues, 1) - 1 ;
  lastSectionForSegment = [];
  %lastSectionForSegment = firstSectionForSegment + ...
  %  find(cSectionLengths > curSF*nrvHdr.Segments(segment).duration,1) - 2 ;

  if isempty(lastSectionForSegment)
    lastSectionForSegment = length(cSectionLengths);
  end

  offsetSectionLengths = cSectionLengths - cSectionLengths(firstSectionForSegment);

  firstSection = find(offsetSectionLengths < range(1) ,1,'last');

  samplesInChannel = nrvHdr.Segments(segment).sampleCount;
  if range(2) > samplesInChannel
    endRange = samplesInChannel;
  else
    endRange = range(2);
  end

  lastSection = find(offsetSectionLengths >= endRange,1)-1;

  if isempty(lastSection)
    lastSection = length(offsetSectionLengths)-1;
  end

  if lastSection > lastSectionForSegment
    ft_error('Index out of range for current section: %i > %i, on channel: %i', ...
      range(2), cSectionLengths(lastSectionForSegment+1), chIdx(i));
  end

  useSections = allSections(firstSection: lastSection);
  useSectionL = sectionLengths(firstSection: lastSection);

  % First Partial Segment
  curIdx = 1;
  curSec = nrvHdr.MainIndex(useSections(1));
  fseek(h, curSec.offset,'bof');

  firstOffset = range(1) - offsetSectionLengths(firstSection);
  lastOffset = min([range(2) useSectionL(1)]);
  lsec = lastOffset-firstOffset + 1;

  fseek(h, (firstOffset-1) * 2,'cof');
  out(1 : lsec,i) = fread(h, lsec, 'int16') * mult;
  curIdx = curIdx +  lsec;

  if length(useSections) > 1
    % Full Segments
    for j = 2: (length(useSections)-1)
      curSec = nrvHdr.MainIndex(useSections(j));
      fseek(h, curSec.offset,'bof');

      out(curIdx : (curIdx + useSectionL(j) - 1),i) = ...
        fread(h, useSectionL(j), 'int16') * mult;
      curIdx = curIdx +  useSectionL(j);
    end

    % Final Partial Segment
    curSec = nrvHdr.MainIndex(useSections(end));
    fseek(h, curSec.offset,'bof');
    %Sometimes there is less data available in the file.
    %So we calculate the wanted lastReadLength,
    %but assign in the data array the actualReadLength
    lastReadLength = length(out)-curIdx+1;
    %disp(['LastReadLength: ' num2str(lastReadLength)]);
    lastData = fread(h, lastReadLength, 'int16');
    actualReadLength = length(lastData);
    %if (actualReadLength <lastReadLength)
    %  warning('Some data not found');
    %disp(['ActualLastReadLength: ' num2str(lastReadLength)]);
    out(curIdx : (curIdx+actualReadLength-1),i) = lastData * mult;
  end

end

% Close the .e file.
fclose(h);

end


function ticklabelformat(hAxes,axName,format)
%TICKLABELFORMAT  Sets axis tick labels format
%
% TICKLABELFORMAT enables setting the format of the tick labels
%
% Syntax:
%    ticklabelformat(hAxes,axName,format)
%
% Input Parameters:
%    hAxes  - handle to the modified axes, such as returned by the gca function
%    axName - name(s) of axles to modify: 'x','y','z' or combination (e.g. 'xy')
%    format - format of the tick labels in sprintf format (e.g. '%.1f V') or a
%             function handle that will be called whenever labels need to be updated
%
%    Note: Calling TICKLABELFORMAT again with an empty ([] or '') format will revert
%    ^^^^  to Matlab's normal tick labels display behavior
%
% Examples:
%    ticklabelformat(gca,'y','%.6g V') - sets y axis on current axes to display 6 significant digits
%    ticklabelformat(gca,'xy','%.2f')  - sets x & y axes on current axes to display 2 decimal digits
%    ticklabelformat(gca,'z',@myCbFcn) - sets a function to update the Z tick labels on current axes
%    ticklabelformat(gca,'z',{@myCbFcn,extraData}) - sets an update function as above, with extra data
%
% Warning:
%    This code heavily relies on undocumented and unsupported Matlab functionality.
%    It works on Matlab 7+, but use at your own risk!
%
% Technical description and more details:
%    http://UndocumentedMatlab.com/blog/setting-axes-tick-labels-format/
%
% Bugs and suggestions:
%    Please send to Yair Altman (altmany at gmail dot com)
%
% Change log:
%    2012-04-18: first version
%
% See also: sprintf, gca

    % Check # of args (we now have narginchk but this is not available on older Matlab releases)
    if nargin < 1
        help(mfilename)
        return;
    elseif nargin < 3
        error('YMA:TICKLABELFORMAT:ARGS','Not enough input arguments');
    end

    % Check input args
    if ~ishandle(hAxes) || ~isa(handle(hAxes),'axes')
        error('YMA:TICKLABELFORMAT:hAxes','hAxes input argument must be a valid axes handle');
    elseif ~ischar(axName)
        error('YMA:TICKLABELFORMAT:axName','axName input argument must be a string');
    elseif ~isempty(format) && ~ischar(format) && ~isa(format,'function_handle') && ~iscell(format)
        error('YMA:TICKLABELFORMAT:format','format input argument must be a string or function handle');
    end

    % normalize axes name(s) to lowercase
    axName = lower(axName);

    if strfind(axName,'x')
        install_adjust_ticklbl(hAxes,'X',format)
    end
    if strfind(axName,'y')
        install_adjust_ticklbl(hAxes,'Y',format)
    end
    if strfind(axName,'z')
        install_adjust_ticklbl(hAxes,'Z',format)
    end

% Install the new tick labels for the specified axes
function install_adjust_ticklbl(hAxes,axName,format)

    % If empty format was specified
    if isempty(format)
        % Remove the current format (revert to default Matlab format)
        set(hAxes,[axName 'TickLabelMode'],'auto')
        setappdata(hAxes,[axName 'TickListener'],[])
        return
    end

    % Determine whether to use the specified format as a
    % sprintf format or a user-specified callback function
    if ischar(format)
        cb = {@adjust_ticklbl axName format};
    else
        cb = format;
    end

    % Now install axis tick listeners to adjust tick labels
    % (use undocumented feature for adjustments)
    ha = handle(hAxes);
    hp = findprop(ha,[axName 'Tick']);
    hl = handle.listener(ha,hp,'PropertyPostSet',cb);
    setappdata(hAxes,[axName 'TickListener'],hl)

    % Adjust tick labels now
    %eventData.AffectedObject = hAxes;
    %adjust_ticklbl([],eventData,axName,format)
    set(hAxes,[axName 'TickLabelMode'],'manual')
    set(hAxes,[axName 'TickLabelMode'],'auto')

% Default tick labels update callback function (used if user did not specify their own function)
function adjust_ticklbl(hProp,eventData,axName,format)	%#ok<INUSL>
    hAxes = eventData.AffectedObject;
    tickValues = get(hAxes,[axName 'Tick']);
    tickLabels = arrayfun(@(x)(sprintf(format,x)),tickValues,'UniformOutput',false);
    set(hAxes,[axName 'TickLabel'],tickLabels)


function disp(L)
%DISP   Pretty-print a linop summary.
%
% See also DISPLAY.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Obtain size of the linop
[m, n] = size(L);

% Determine whether format is compact or loose:
loose = ~isequal(get(0, 'FormatSpacing'), 'compact');

if ( loose )
    fprintf('\n');
end

fprintf('   %ix%i linear operator', m,n)

if ( loose )
    fprintf('\n');
end

nc = size(L.constraint.functional, 1);
if ( nc == 1 )
    fprintf('\n   with 1 constraint/boundary condition.')
elseif ( nc > 0 )
    fprintf('\n   with %i constraints/boundary conditions.',nc)
end

if ( loose )
    fprintf('\n');
end

fprintf('\n');

end


function  [frames, names, height,width,nframe ] = readAllFrames(options)

    frameFiles = imdir(fullfile(options.infolder));
    nframe = length( frameFiles);
    frames = cell( nframe, 1 );
    names = cell( nframe, 1 );
    for index = 1: nframe
        [~, frameName] = fileparts(frameFiles(index).name);
        if exist(fullfile(options.infolder, [frameName '.png']),'file')
            frame = imread(fullfile(options.infolder, [frameName '.png']));
        elseif exist(fullfile(options.infolder, [frameName '.jpg']),'file')
            frame = imread(fullfile(options.infolder, [frameName '.jpg']));
        elseif exist(fullfile(options.infolder, [frameName '.bmp']),'file')
            frame = imread(fullfile(options.infolder, [frameName '.bmp']));
        end
        frames{ index } = double(frame);
        names{ index } =  frameName;
    end
    [ height,width ] = size(frame(:,:,1));
end