function varargout = dim(varargin)
% file_array's dimension property
% For getting the value
% dat = dim(obj)
%
% For setting the value
% obj = dim(obj,dat)
%__________________________________________________________________________
% Copyright (C) 2005-2017 Wellcome Trust Centre for Neuroimaging

%
% $Id: dim.m 7147 2017-08-03 14:07:01Z spm $

if nargin==2
    varargout{1} = asgn(varargin{:});
elseif nargin==1
    varargout{1} = ref(varargin{:});
else
    error('Wrong number of arguments.');
end

%==========================================================================
% function dat = ref(obj)
%==========================================================================
function dat = ref(obj)
dat = obj.dim;

%==========================================================================
% function obj = asgn(obj,dat)
%==========================================================================
function obj = asgn(obj,dat)
if isnumeric(dat) && all(dat>=0) && all(rem(dat,1)==0)
    dat = [double(dat(:)') 1 1];
    lim = max([2 find(dat~=1)]);
    dat = dat(1:lim);
    obj.dim = dat;
else
    error('"dim" must be a vector of positive integers.');
end


function result = set_joinepos(varargin)
% Join epoched signals across epochs.
% Result = set_joinepos(Set1, Set2, ...)
%
% In:
%   SetK   : The k'th data set to join.
%
% Out:
%   Result : A new data set with trials from all sets joined. The following changes are made:
%            * .data and all other time-series fields are joined across trials (3rd dimension)
%            * .epoch is joined and its .event field is updated appropriately
%            * .event is joined and its .epoch and .latency fields are updated appropriately
%            * .xmax/.trials are updated
%
% Notes:
%   This function returns a new data set with meta-data set to that of the first input set, and the
%   trials joined across all sets. No checks for meta-data consistency are done. There is a
%   heavy-duty function for merging inconsistent sets called set_merge, which can merge cats and
%   dogs. This function does not attempt to keep miscellaneous EEGLAB meta-data consistent, including:
%   setname,filename,filepath,subject,group,condition,session,comments,urevent,reject,stats,history,etc
%
%   To concatenate continuous sets across time, use set_concat.
%
% Examples:
%   % merge data sets eegA, eegB and eegC across epochs
%   eeg = set_joinepos(eegA,eegB,eegC)
%
% See also:
%   set_concat, set_merge
%
%                                Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                                2010-03-31
dp;

% set_joinepos_version<1.0> -- for the cache

if ~exp_beginfun('editing') return; end

declare_properties('name','JoinTrials','independent_channels',true,'independent_trials',true);

% input validation
for k=1:length(varargin)
    utl_check_fields(varargin{k},{'data','event','epoch','pnts','xmin','xmax','srate'},'input','signal');
    if ~isempty(varargin{k}.event)
        if ~isfield(varargin{k}.event,'latency')
            error('A dataset passed to set_joineos is lacking the .event.latency field.'); end
        if ~isfield(varargin{k}.event,'epoch')
            error('A dataset passed to set_joineos is lacking the .event.epoch field.'); end
        latency_numels = cellfun('prodofsize',{varargin{k}.event.latency});
        if any(latency_numels == 0)
            error('One or more of the events in the given data set have an empty .latency field, which is not permitted.'); end
        if any(latency_numels ~= 1)
            error('One or more of the events in the given data set have a .latency value that is not a scalar, which is not permitted.'); end
    end
    if ~isempty(varargin{k}.epoch)
        if ~isfield(varargin{k}.epoch,'event')
            error('A dataset passed to set_joineos is lacking the .epoch.event field.'); end
        epoch_numels = cellfun('prodofsize',{varargin{k}.event.epoch});
        if any(epoch_numels == 0)
            error('One or more of the events in the given data set have an empty .epoch field, which is not permitted.'); end
        if any(epoch_numels ~= 1)
            error('One or more of the events in the given data set have an .epoch value that is not a scalar, which is not permitted.'); end
    end
end

if ~isempty(varargin)
    result = varargin{1};
    if length(varargin) > 1
        % concatenate time series fields
        for field = utl_timeseries_fields(result)
            data = cellfun(@(x)x.(field{1}),varargin,'UniformOutput',false);
            try
                result.(field{1}) = cat(3,data{:});
            catch e
                % concatenation failed: produce a reasonable error message
                if ~isempty(data)
                    sizes1 = cellfun('size',data,1); sizes1 = sizes(sizes1 ~= 0);
                    sizes2 = cellfun('size',data,2); sizes2 = sizes(sizes2 ~= 0);
                    if length(sizes1) > 1 && ~all(sizes1==sizes1(1))
                        error('The time-series field .%s must have the same number of channels in each data set.',field{1}); end
                    if length(sizes2) > 1 && ~all(sizes2==sizes2(1))
                        error('The time-series field .%s must have the same number of tine points in each data set.',field{1}); end
                end
                size_info = hlp_tostring(cellfun(@size,data,'UniformOutput',false));
                error('Concatenation of time-series fields failed with error: %s (the data sizes were %s -- make sure that they are mutually compatible).',e.message,size_info);
            end
            if isempty(result.(field{1}))
                result.(field{1}) = []; end
        end
        % count events, epochs and samples in each set
        event_count = cellfun(@(x)length(x.event),varargin);
        epoch_count = cellfun(@(x)length(x.epoch),varargin);
        sample_count = cellfun(@(x)x.pnts,varargin).*epoch_count;
        % concatenate .event and .epoch fields
        event = cellfun(@(x)x.event,varargin,'UniformOutput',false); result.event = [event{:}];
        epoch = cellfun(@(x)x.epoch,varargin,'UniformOutput',false); result.epoch = [epoch{:}];
        % shift event latencies based on cumulative sample counts
        if ~isempty(result.event)
            [result.event.latency] = arraydeal([result.event.latency]+replicate(cumsum(sample_count)-sample_count,event_count));
            % shift event/epoch cross-references based on cumulative counts
            [result.event.epoch] = arraydeal([result.event.epoch]+replicate(cumsum(epoch_count)-epoch_count,event_count));
            [result.epoch.event] = chopdeal([result.epoch.event]+replicate(cumsum(event_count)-event_count,event_count),cellfun('length',{result.epoch.event}));
        end
        % update misc fields
        [result.nbchan,result.pnts,result.trials,extra_dims] = size(result.data); %#ok<NASGU>
        result.xmax = result.xmin + (result.pnts-1)/result.srate;
    end
else
    result = struct('setname','','filename','','filepath','','subject','','group','','condition','','session',[],'comments','','nbchan',0,...
        'trials',0,'pnts',0,'srate',1,'xmin',0,'xmax',0,'times',[],'data',[],'icaact',[],'icawinv',[],'icasphere',[],'icaweights',[], ...
        'icachansind',[],'chanlocs',[],'urchanlocs',[],'chaninfo',[],'ref',[],'event',[],'urevent',[],'eventdescription',{{}}, ...
        'epoch',[],'epochdescription',{{}},'reject',[],'stats',[],'specdata',[],'specicaact',[],'splinefile','','icasplinefile','', ...
        'dipfit',[],'history','','saved','no','etc',[]);
end

exp_endfun;

function result = replicate(values,counts)
% Replicate each element Values(k) by Count(k) times.
result = zeros(1,sum(counts));
k = 0;
for p=find(counts)
    result(k+(1:counts(p))) = values(p);
    k = k+counts(p);
end


function bool = index_selected(iter, indeces)

if islogical(indeces)
  if isscalar(indeces)
    bool = indeces;
  else
    error('Logical values must not be matrices');
  end
elseif isscalar(indeces)
  if indeces > 0
    bool = iter >= indeces;
  else
    error('Index must be positive integer.');
  end
else
  bool = any(iter==indeces);
end


function i4mat_write ( output_filename, m, n, table )

%*****************************************************************************80
%
%% I4MAT_WRITE writes an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string OUTPUT_FILENAME, the output filename.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, integer TABLE(M,N), the points.
%
%    Input, logical HEADER, is TRUE if the header is to be included.
%

%
%  Open the file.
%
  output_unit = fopen ( output_filename, 'wt' );

  if ( output_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_WRITE - Error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'I4MAT_WRITE - Error!' );
    return;
  end
%
%  Write the data.
%
  for j = 1 : n
    for i = 1 : m
      fprintf ( output_unit, '  %12d', round ( table(i,j) ) );
    end
    fprintf ( output_unit, '\n' );
  end
%
%  Close the file.
%
  fclose ( output_unit );

  return
end


%% This function will exclude the left hand side guess from the right hand side library.
% Last Updated: 2019/04/22
% Coded By: K
function [RHS_Data,RHS_Struct]=ExcludeGuess(SINDy_Data,SINDy_Struct,LHS_Sym)
% First get the size of the library
[n,m]=size(SINDy_Data);
% Then run a for loop and generate a new library
j=1;
for i=1:m
    if SINDy_Struct{i}~=LHS_Sym
        RHS_Data(:,j)=SINDy_Data(:,i);
        RHS_Struct(1,j)=SINDy_Struct(1,i);
        j=j+1;
    else
        % For use of debuging
        SINDy_Struct{i};
    end
end


function b = iscellnumeric(C)
% Return 1 if all elements of cell array are numeric
%
% Tim Mullen, 2011, SCCN/INC, UCSD

b = all(cellfun(@(x) isnumeric(x),C));


function [ A ] = opt_catStruct( varargin )
% CATSTRUCT   Concatenate or merge structures with different fieldnames
%   X = CATSTRUCT(S1,S2,S3,...) merges the structures S1, S2, S3 ...
%   into one new structure X. X contains all fields present in the various
%   structures. An example:
%
%     A.name = 'Me' ;
%     B.income = 99999 ;
%     X = catstruct(A,B)
%     % -> X.name = 'Me' ;
%     %    X.income = 99999 ;
%
%   If a fieldname is not unique among structures (i.e., a fieldname is
%   present in more than one structure), only the value from the last
%   structure with this field is used. In this case, the fields are
%   alphabetically sorted. A warning is issued as well. An axample:
%
%     S1.name = 'Me' ;
%     S2.age  = 20 ; S3.age  = 30 ; S4.age  = 40 ;
%     S5.honest = false ;
%     Y = catstruct(S1,S2,S3,S4,S5) % use value from S4
%
%   The inputs can be array of structures. All structures should have the
%   same size. An example:
%
%     C(1).bb = 1 ; C(2).bb = 2 ;
%     D(1).aa = 3 ; D(2).aa = 4 ;
%     CD = catstruct(C,D) % CD is a 1x2 structure array with fields bb and aa
%
%   The last input can be the string 'sorted'. In this case,
%   CATSTRUCT(S1,S2, ..., 'sorted') will sort the fieldnames alphabetically.
%   To sort the fieldnames of a structure A, you could use
%   CATSTRUCT(A,'sorted') but I recommend ORDERFIELDS for doing that.
%
%   When there is nothing to concatenate, the result will be an empty
%   struct (0x0 struct array with no fields).
%
%   NOTE: To concatenate similar arrays of structs, you can use simple
%   concatenation:
%     A = dir('*.mat') ; B = dir('*.m') ; C = [A ; B] ;

%   NOTE: This function relies on unique. Matlab changed the behavior of
%   its set functions since 2013a, so this might cause some backward
%   compatibility issues when dulpicated fieldnames are found.
%
%   See also CAT, STRUCT, FIELDNAMES, STRUCT2CELL, ORDERFIELDS

% version 4.1 (feb 2015), tested in R2014a
% Copyright (c) 2015, Jos van der Geest
% All rights reserved.
% (c) Jos van der Geest
% email: jos@jasen.nl

% History
% Created in 2005
% Revisions
%   2.0 (sep 2007) removed bug when dealing with fields containing cell
%                  arrays (Thanks to Rene Willemink)
%   2.1 (sep 2008) added warning and error identifiers
%   2.2 (oct 2008) fixed error when dealing with empty structs (thanks to
%                  Lars Barring)
%   3.0 (mar 2013) fixed problem when the inputs were array of structures
%                  (thanks to Tor Inge Birkenes).
%                  Rephrased the help section as well.
%   4.0 (dec 2013) fixed problem with unique due to version differences in
%                  ML. Unique(...,'last') is no longer the deafult.
%                  (thanks to Isabel P)
%   4.1 (feb 2015) fixed warning with narginchk

narginchk(1,Inf) ;
N = nargin ;

if ~isstruct(varargin{end}),
    if isequal(varargin{end},'sorted'),
        narginchk(2,Inf) ;
        sorted = 1 ;
        N = N-1 ;
    else
        error('catstruct:InvalidArgument','Last argument should be a structure, or the string "sorted".') ;
    end
else
    sorted = 0 ;
end

sz0 = [] ; % used to check that all inputs have the same size

% used to check for a few trivial cases
NonEmptyInputs = false(N,1) ;
NonEmptyInputsN = 0 ;

% used to collect the fieldnames and the inputs
FN = cell(N,1) ;
VAL = cell(N,1) ;

% parse the inputs
for ii=1:N,
    X = varargin{ii} ;
    if ~isstruct(X),
        error('catstruct:InvalidArgument',['Argument #' num2str(ii) ' is not a structure.']) ;
    end

    if ~isempty(X),
        % empty structs are ignored
        if ii > 1 && ~isempty(sz0)
            if ~isequal(size(X), sz0)
                error('catstruct:UnequalSizes','All structures should have the same size.') ;
            end
        else
            sz0 = size(X) ;
        end
        NonEmptyInputsN = NonEmptyInputsN + 1 ;
        NonEmptyInputs(ii) = true ;
        FN{ii} = fieldnames(X) ;
        VAL{ii} = struct2cell(X) ;
    end
end

if NonEmptyInputsN == 0
    % all structures were empty
    A = struct([]) ;
elseif NonEmptyInputsN == 1,
    % there was only one non-empty structure
    A = varargin{NonEmptyInputs} ;
    if sorted,
        A = orderfields(A) ;
    end
else
    % there is actually something to concatenate
    FN = cat(1,FN{:}) ;
    VAL = cat(1,VAL{:}) ;
    FN = squeeze(FN) ;
    VAL = squeeze(VAL) ;

    [UFN,ind] = unique(FN, 'last') ;
    % If this line errors, due to your matlab version not having UNIQUE
    % accept the 'last' input, use the following line instead
    % [UFN,ind] = unique(FN) ; % earlier ML versions, like 6.5

    if numel(UFN) ~= numel(FN),
%         warning('catstruct:DuplicatesFound','Fieldnames are not unique between structures.') ;
        sorted = 1 ;
    end

    if sorted,
        VAL = VAL(ind,:) ;
        FN = FN(ind,:) ;
    end

    A = cell2struct(VAL, FN);
    A = reshape(A, sz0) ; % reshape into original format

end


function [err, hndl] = addsquare(Pll, Pur, C)
%
% [err, hndl] = addsquare(Pll, Pur, C)
%
%
%Purpose:
%  adds a rectangle on current plot
%
%
%
%Input Parameters:
%  Pll : [x y] of lower left point
%  Pur : [x y] of upper right point
%  C  : rgb triplet, between 0 and 1  rectangle color
%
%
%
%Output Parameters:
%   err : 0 No Problem
%       : 1 Mucho Problems
%   hndl : the handle to the rectangle object
%
%
%Key Terms:
%
%More Info :
%
%   add the square first, before making the plots,
%   otherwise, the patch will obscure the plots
%
%
%     Author : Ziad Saad
%     Date : Fri Oct 06 15:47:14 EDT 2000
%     LBC/NIMH/ National Institutes of Health, Bethesda Maryland

%Define the function name for easy referencing
FuncName = 'addsquare';

%Debug Flag
DBG = 1;

%initailize return variables
err = 1;

X = [Pll(1) Pur(1) Pur(1) Pll(1)]';
Y = [Pll(2) Pll(2) Pur(2) Pur(2)]';

hold on;
hndl = patch(X,Y, C);
set(hndl, 'EdgeColor', C);

err = 0;
return;


classdef nncycle < nntest
  methods (Test)
    function testCycle(test)
      % diamond topology
      a = Input() ;
      b = sqrt(a) ;
      c = abs(a) ;
      d = b + c ;
      e = exp(d) ;

      % introduce cycles in the DAG
      try
        b.inputs{1} = e ;
      catch err
        assert(strcmp(err.identifier, 'MatConvNet:CycleCheckFailed'), ...
          'Failed to catch cycle in DAG.') ;
      end

      try
        d.inputs{2} = d ;
      catch err
        assert(strcmp(err.identifier, 'MatConvNet:CycleCheckFailed'), ...
          'Failed to catch cycle in DAG.') ;
      end
    end
  end
end


function [modifiedMap, xmlMap, model2] = setColorToMapRxns (mapFileName, model, v1, rxnsFluxTask, rxnLineWidth, rxnLineColor, molFillColor)
% USAGE:
% [modifiedMap, xmlMap, model2] =
%   setColorToMapRxns (mapFileName, model, v1, rxnsFluxTask, rxnLineWidth, rxnLineColor, molFillColor)
%
% INPUTS:
% mapFileName           relative path to XML map (e.g. 'yeastMap1.0.xml')
% model                 model structure
% v1                    vector of fluxes
% rxnsFluxTask          1 = Flux
%                       2 = Flux essentiality; map reactions causing any
%                           effect on growth (i.e. obj function)
%                       3 = Flux essentiality; map reactions causing no
%                           growth(i.e. obj function) when deleted, i.e.
%                           essential reactions
%                       4 = Flux essentiality; map reactions with not any
%                           effect on growth (i.e. obj function) when
%                           deleted
%                       5 = Omics
% ADDITIONAL INPUTS:
% rxnLineWidth          reaction line width; default = 5
% rxnLineColor          reaction line color; default = Pathways; write
%                       'Subsystems' to color five subsystems only (central,
%                       AA, lipids, cofactors, nuclotides and energy).
%                       'Subsystems' option is the deault option at task 5
% molFillColor          molecule fill color; default = true (i.e. white)
%                       molecule fill color; true (i.e. like CellDesigner default)
% OUTPUTS:
% modifiedMap           map with MATLAB structure, and modified reactions' line color and width
% xmlMap                xml structure of the original map obtained from mapFileName
% model2                model with modified subsystems

if ~(exist('transformXML2Map')==2)
    error('COBRA Toolbox is required, go to https://opencobra.github.io/cobratoolbox/')
end

if nargin < 5 || rxnLineWidth == 0
    rxnLineWidth = 0;
    rxnLineColor = '';
    molFillColor = true;
elseif nargin < 6
    rxnLineColor = '';
    molFillColor = true;
elseif nargin < 7
    molFillColor = true;
end
if rxnsFluxTask == 5
    rxnLineColor = 'Subsystems';
end

[xmlMap, mapMap] = transformXML2Map(mapFileName);
if molFillColor == true
    modifiedMap = unifyMetabolicMapCD(mapMap); % molColor whrite and rxnColor = ligthgray
elseif molFillColor == false
    modifiedMap = mapMap; % like CellDesigner default
end

%%%%%%%% Gettin reactions line width with normalized fluxes %%%%%%%%%%%%%
model2 = model; % to not overwrite model
flxs(:, 1) = v1; % to not overwrite v1
absFlxs = abs(flxs);
rxnLineWidthInMap = absFlxs / max(absFlxs); %normalize fluxes line width
if rxnsFluxTask == 1 && rxnLineWidth == 0
    rxnLineWidthInMap(rxnLineWidthInMap > 0.1 & rxnLineWidthInMap <=  1) = 50;
    rxnLineWidthInMap(rxnLineWidthInMap > 0.01 & rxnLineWidthInMap <= 0.1) = 25;
    rxnLineWidthInMap(rxnLineWidthInMap > 0.001 & rxnLineWidthInMap <= 0.01) = 16;
    rxnLineWidthInMap(rxnLineWidthInMap > 0.0001 & rxnLineWidthInMap <= 0.001) = 10;
    rxnLineWidthInMap(rxnLineWidthInMap > 0.00001 & rxnLineWidthInMap <= 0.0001) = 6;
elseif rxnsFluxTask == 2 && rxnLineWidth == 0
    rxnLineWidthInMap(rxnLineWidthInMap > 0.90 & rxnLineWidthInMap <=  0.95) =  6;
    rxnLineWidthInMap(rxnLineWidthInMap > 0.80 & rxnLineWidthInMap <= 0.90) =  8;
    rxnLineWidthInMap(rxnLineWidthInMap > 0.50 & rxnLineWidthInMap <= 0.80) = 11;
    rxnLineWidthInMap(rxnLineWidthInMap > 0.25 & rxnLineWidthInMap <= 0.50) = 15;
    rxnLineWidthInMap(rxnLineWidthInMap >= 0.00 & rxnLineWidthInMap <= 0.25) = 20;
elseif rxnsFluxTask == 1 && rxnLineWidth ~= 0.0
    rxnLineWidthInMap(rxnLineWidthInMap == 1.0) = rxnLineWidth;
elseif rxnsFluxTask == 2 && rxnLineWidth ~= 0.0
    rxnLineWidthInMap(rxnLineWidthInMap < 0.95) = rxnLineWidth;
elseif rxnsFluxTask == 3 && rxnLineWidth == 0.0
    rxnLineWidthInMap(rxnLineWidthInMap == 0.0) = 5;
elseif rxnsFluxTask == 3 && rxnLineWidth ~= 0.0
    rxnLineWidthInMap(rxnLineWidthInMap == 0.0) = rxnLineWidth;
elseif rxnsFluxTask == 4 && rxnLineWidth == 0.0
    rxnLineWidthInMap(rxnLineWidthInMap > 0.95 & rxnLineWidthInMap <= 1.0) = 5.0;
elseif rxnsFluxTask == 4 && rxnLineWidth ~= 0.0
    rxnLineWidthInMap(rxnLineWidthInMap > 0.95 & rxnLineWidthInMap <= 1.0) = rxnLineWidth;
elseif rxnsFluxTask == 5 && rxnLineWidth == 0.0
    cutValue = 2.0; % transcriptomics minimum cut value of log change
    rxnLineWidthInMap = absFlxs;
    rxnLineWidthInMap(rxnLineWidthInMap > 5.0) = 30;
    rxnLineWidthInMap(rxnLineWidthInMap > 4.0 & rxnLineWidthInMap <= 5.0) = 25;
    rxnLineWidthInMap(rxnLineWidthInMap > 3.0 & rxnLineWidthInMap <= 4.0) = 20;
    rxnLineWidthInMap(rxnLineWidthInMap > 2.0 & rxnLineWidthInMap <= 3.0) = 15;
    rxnLineWidthInMap(rxnLineWidthInMap > 1.0 & rxnLineWidthInMap <= 2.0) = 10;
    rxnLineWidthInMap(rxnLineWidthInMap >= 0.5 & rxnLineWidthInMap <= 1.0) =  5;
end
for i = 1:length(modifiedMap.rxnName)
    a = find(ismember(model2.rxns, modifiedMap.rxnName{i}));
    if isempty(a)
        modifiedMap.rxnWidth{i} = 1;
    else
        modifiedMap.rxnWidth{i} = rxnLineWidthInMap(a);
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%% Cofactors not included in the analyses %%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%% Compartments included in the analyses %%%%%%%%%%%%%%%%%%%%%%%%%

% propossed cofactors list for PART A
cofactorsListA ={'h'; 'h2o'; 'nad'; 'nadh'; 'nadp'; 'nadph';...
    'fad'; 'fadh'; 'ppi'; 'adp'; 'gtp'; 'gdp'; 'gmp'; ...
    'utp'; 'udp'; 'ump'; 'ctp'; 'cdp'; 'cmp'};

% propossed cofactors list for PART B
cofactorsListB ={'h'; 'h2o'; 'co2'; 'coa'; 'accoa'; 'nad'; 'nadh'; 'nadp'; 'nadph';...
    'fad'; 'fadh'; 'pi'; 'ppi'; 'atp'; 'adp'; 'amp'; 'gtp'; 'gdp'; 'gmp';...
    'utp'; 'udp'; 'ump'; 'ctp'; 'cdp'; 'cmp'; 'gthox'; 'gthrd_c'; 'amet_c';...
    'ahcys_c'; 'glu__L'; 'gln__L'; 'akg'; 'nh4'};

% compartments list
compartmentsList = {'_e'; '_c'; '_m'; '_n'; '_x'; '_v'; '_r'; '_g'};

% include compartment to cofactor lists A and B
cofactorsListWcompA = cell(length(cofactorsListA), 1);
c = 1; % general counter
for i = 1:length(cofactorsListA)
    for j = 1:length(compartmentsList)
        cofactorsListWcompA{c, 1} = horzcat(cofactorsListA{i},compartmentsList{j});
        c = c + 1;
    end
end
cofactorsListWcompB = cell(length(cofactorsListB), 1);
c = 1;
for i = 1:length(cofactorsListB)
    for j = 1:length(compartmentsList)
        cofactorsListWcompB{c, 1}= horzcat(cofactorsListB{i},compartmentsList{j});
        c = c + 1;
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%% PART A %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% getting transport reactions
[nrxns,~] = size(model2.rxns);
transRxns = cell(nrxns, 1);
for j = 1:size(model2.rxns)
    if true(contains(model2.subSystems {j}, 'S_Transport'))
        transRxns{j, 1} = model2.rxns{j};
    end
end
transRxns = transRxns(~cellfun('isempty',transRxns));

% getting subStystems without transport
SubSystems = (unique(getModelSubSystems(model2)));
[nSubSystems, ~] = size(SubSystems);
subSystemsNoTrans = cell(nSubSystems, 1);
for i = 1:nSubSystems
    if ~contains(SubSystems{i,1},'S_Transport')&& ...
            ~contains(SubSystems{i, 1},'Extracellular exchange') &&...
            ~contains(SubSystems{i, 1},'S_tRNA_charging')
        subSystemsNoTrans{i, 1} = SubSystems{i, 1};
    else
        subSystemsNoTrans{i, 1} = {};
    end
end
subSystemsNoTrans = subSystemsNoTrans(~cellfun('isempty',subSystemsNoTrans));

% defining subSystems of trasnport reactions to be colored, these
% transport reactions complete reaction conections in subSystems which
% require conexions among diferents compartments
[nTransRxns,~] = size(transRxns);
[nSubSystemsNoTrans, ~] = size (subSystemsNoTrans);
totalRxnsToIncludeA = cell(nTransRxns,1);
for i = 1:nSubSystemsNoTrans
    subSystemRxns = findRxnsFromSubSystem(model2, subSystemsNoTrans(i));
    metsSubSystemRxns = findMetsFromRxns(model2, subSystemRxns);
    [nMetsSubSystemRxns, ~] = size (metsSubSystemRxns);
    % getting unique compartments
    metsCompartment = cell(nMetsSubSystemRxns,1);
    for j = 1:nMetsSubSystemRxns
        str = metsSubSystemRxns{j};
        [~, s] = size(str);
        metsCompartment{j,1} = str(s-1:s);
    end
    metsCompartment = (unique(metsCompartment));
    [nUniqueMetsComp, ~] = size(metsCompartment);

    if nUniqueMetsComp > 1
        % getting duplicated mets like 'pyr_c' and 'pyr_m' which are 'pyr'
        % and pyr after erasing the compartment signature
        metsFromSubSystemRxnsNoComp = cell(nMetsSubSystemRxns,1);
        for j = 1:nMetsSubSystemRxns
            str = metsSubSystemRxns{j};
            [z, s] = size(str);
            metsFromSubSystemRxnsNoComp{j, 1} = str(1:s-2); % erasing comp signature
        end
        [~, o, ~] = unique(metsFromSubSystemRxnsNoComp,'first');
        indexToDupes = find(not(ismember(1:numel(metsFromSubSystemRxnsNoComp),o)));
        [~, nIndexToDupes] = size(indexToDupes);
        duplicatedMets = cell(nIndexToDupes,1);
        for j = 1:nIndexToDupes
            duplicatedMets(j, 1) = metsFromSubSystemRxnsNoComp (indexToDupes(1, j));
        end

        % getting reactions from duplicated mets
        dulicatedRxnMets = cell(nIndexToDupes * nUniqueMetsComp,1);
        c = 1; % duplicatedRxnMets counter
        for j = 1:nIndexToDupes
            str = duplicatedMets{j, 1};
            if ~ismember(str, cofactorsListA)
                for k = 1:nUniqueMetsComp
                    str1 = metsCompartment{k};
                    if ~ismember(str1, compartmentsList(2)) % compartment '_c'
                        dulicatedRxnMets{c, 1} = {horzcat(str, '_c'), horzcat(str, str1)};
                        c = c + 1;
                    end
                end
            end
        end
        dulicatedRxnMets = dulicatedRxnMets(~cellfun('isempty',dulicatedRxnMets));
        [ndulicatedRxnMets, ~] = size(dulicatedRxnMets);
        % from duplicated reactions, get reactions to subscribe a reactions
        % subsystem from non transport subsystems
        rxnsToInclude = cell(ndulicatedRxnMets, 1);
        for j = 1:ndulicatedRxnMets
            metsToFindInTrasnportRxns = dulicatedRxnMets{j, 1};
            for k = 1:nTransRxns
                rxnFormula = printRxnFormula(model2, transRxns{k, 1}, false);
                if true(contains(string(rxnFormula), metsToFindInTrasnportRxns (1, 1)))
                    if true(contains(rxnFormula, metsToFindInTrasnportRxns (1, 2)))
                        rxnsToInclude{j, 1} = transRxns{k, 1};
                    end
                end
            end
        end
        rxnsToInclude = rxnsToInclude(~cellfun('isempty',rxnsToInclude));
        rxnsToInclude = unique(rxnsToInclude);
        [s, ~] = size(rxnsToInclude);
        totalRxnsToIncludeA = [totalRxnsToIncludeA; rxnsToInclude];

        for j = 1:s
            rxnID = findRxnIDs(model2, rxnsToInclude{j});
            model2.subSystems(rxnID) = {subSystemsNoTrans(i)};
        end

    end
end
totalRxnsToIncludeA = totalRxnsToIncludeA(~cellfun('isempty',totalRxnsToIncludeA));
totalRxnsToIncludeA = unique(totalRxnsToIncludeA);

%%%%%%%%%%%%%%%%%%%%%%%%%%%% PART B %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Get subsystems category. This is based on the number of conexions that a
% subsystem includes in the genome scale metabolic model2
subsLength ={};
for i = 1:length(subSystemsNoTrans)
    rxnsSubs = findRxnsFromSubSystem(model2,subSystemsNoTrans(i));
    metsSubsRxns = findMetsFromRxns(model2, rxnsSubs);
    metsSubsRxnsNoCof = {};
    for j = 1:length(metsSubsRxns)
        if ~ismember(metsSubsRxns(j),cofactorsListWcompB)
            metsSubsRxnsNoCof{j,1} = char(metsSubsRxns(j));
        end
    end
    metsSubsRxnsNoCof = metsSubsRxnsNoCof(~cellfun('isempty',metsSubsRxnsNoCof));

    metsRxns = findRxnsFromMets(model2, metsSubsRxnsNoCof);
    metsRxnsSubs = {};
    for j = 1:length(metsRxns)
        rxnID = findRxnIDs(model2,metsRxns(j));
        metsRxnsSubs{j,1} = string(model2.subSystems(rxnID));
    end
    metsRxnsSubs = unique(string(metsRxnsSubs));
    subsLength{i,1} = length(metsRxnsSubs);
    subsLength{i,2} = subSystemsNoTrans{i};
end

% Get  remaining transport reacctions, namely, reactions not included in
% PART A

[nTransRxns, ~] = size(transRxns);
remainingTransRxns = cell(nTransRxns, 1);
for i = 1:nTransRxns
    if ~ismember(transRxns(i),totalRxnsToIncludeA)
        remainingTransRxns{i,1} = transRxns{i, 1};
    end
end
remainingTransRxns = remainingTransRxns(~cellfun('isempty',remainingTransRxns));

% Assign remaining transport reactions
nonIncludSubs = {'S_tRNA_charging', 'S_Other',...
    'Biomass and maintenance functions', };
totalRxnsToIncludeAB = totalRxnsToIncludeA;
remTransRxnsWnewSubs = {};
remTransRxnsSubs = {};
rxnsToInclude = cell(length(remainingTransRxns), 1);
subSToInclude = cell(length(remainingTransRxns), 1);
for i = 1:length(remainingTransRxns)
    metsRemTransRxns = findMetsFromRxns(model2, remainingTransRxns(i));
    metsRxns = {};
    for j = 1:length(metsRemTransRxns)
        if true(contains(metsRemTransRxns{j},'_c') &&...
                ~ismember(metsRemTransRxns(j), cofactorsListWcompB))
            metsRxns = findRxnsFromMets(model2,metsRemTransRxns(j));
        end
    end
    metsRxns = metsRxns(~cellfun('isempty',metsRxns));

    rxnSubs = {};
    for j = 1:length(metsRxns)
        rxnID = findRxnIDs(model2, metsRxns(j));
        rxnSubs{j,1} = model2.subSystems{rxnID};
    end
    rxnSubs = rxnSubs(~cellfun('isempty',rxnSubs));

    r = [];
    rankMax = 0;
    subSToUseStr = cell(1,1);
    for j = 1:length(rxnSubs)
        if true(~ismember(string(metsRxns(j)),totalRxnsToIncludeA))
            for k = 1:length(subsLength)
                if true(and(ismember(rxnSubs{j}, subsLength{k,2}),...
                        ~ismember(rxnSubs{j}, nonIncludSubs)))
                    r(k, 1) = subsLength{k, 1};
                    subSToUseStr{1, 1} = string(subsLength(k,2));
                    if k > 1
                        if true(rankMax < r(k, 1))
                            rankMax = r(k, 1);
                            subSToUseStr{1, 1} = string(subsLength(k,2));
                        end
                    end
                end
            end
        end
    end
    if ~isempty(subSToUseStr(1,1))
        rxnsToInclude{i} = remainingTransRxns{i};
        subSToInclude{i} = char(subSToUseStr{1, 1});
        remTransRxnsWnewSubs{i,1} = remainingTransRxns(i);
        remTransRxnsSubs{i,1} = subSToUseStr(1, 1);
        totalRxnsToIncludeAB = [totalRxnsToIncludeAB; rxnsToInclude];
    end
end
totalRxnsToIncludeAB = totalRxnsToIncludeAB(~cellfun('isempty',totalRxnsToIncludeAB));
totalRxnsToIncludeAB = unique(totalRxnsToIncludeAB);

for j = 1:length(rxnsToInclude)
    rxnID = findRxnIDs(model2, rxnsToInclude{j});
    model2.subSystems(rxnID) = {subSToInclude(j)};
end

%%%%%%%%%%%%%%%%%%%%%%%% ASIGN SUBSYSTEMS' COLOR %%%%%%%%%%%%%%%%%%%%%%%%%%%

% Selection of reactions to include in the map
if rxnsFluxTask == 1
    for i = 1:length(model2.rxns)
        if absFlxs(i)==0 % zero flux reactions are not included
            model2.subSystems(i) = {{'NonIncluded'}};  % included at the end
        end
    end
elseif rxnsFluxTask == 2
    for i = 1:length(model2.rxns)
        if absFlxs(i) > 0.95 * max(absFlxs)% reactions causing growth (i.e. obj function) defect when deleting
            model2.subSystems(i) = {{'NonIncluded'}};   % included at the end
        end
    end
elseif rxnsFluxTask == 3
    for i = 1:length(model2.rxns)
        if absFlxs(i) > 0.0 % zero flux reactions only, i.e. essential reations
            model2.subSystems(i) = {{'NonIncluded'}};   % included at the end
        end
    end
elseif rxnsFluxTask == 4
    for i = 1:length(model2.rxns)
        if absFlxs(i) < 0.95 * max(absFlxs)% reactions fluxes causing no growth (i.e. obj function) defect when deleting
            model2.subSystems(i) = {{'NonIncluded'}};   % included at the end
        end
    end
elseif rxnsFluxTask == 5
    for i = 1:length(model2.rxns)
        if absFlxs(i) < cutValue % zero flux reactions are not included
            model2.subSystems(i) = {{'NonIncluded'}};  % included at the end
        end
    end
end

%See colors letter-code at:
%la.mathworks.com/matlabcentral/mlc-downloads/downloads/submissions/24497/versions/2/screenshot.PNG
%[xmlMap, mapMap] = transformXML2Map('YeastMapUnified1.0b.xml');
centralPathsList = {'S_GlycolysisGluconeogenesis', 'S_Pentose_Phosphate_Pathway',...
    'S_Pyruvate_Metabolism', 'S_Citric_Acid_Cycle', 'S_Taurine_Metabolism',...
    'S_Starch_and_Sucrose_Metabolism', 'S_Xylose_Metabolism',...
    'S_Fructose_and_Mannose_Metabolism', 'S_Galactose_metabolism',...
    'S_Arabinose_Metabolism', 'S_Alternate_Carbon_Metabolism', 'S_Methane_Metabolism'};
aminoacPathsList = {'S_Glutamate_metabolism', 'S_Glutamine_Metabolism',...
    'S_Alanine_and_Aspartate_Metabolism', 'S_Histidine_Metabolism',...
    'S_Threonine_and_Lysine_Metabolism', 'S_Glycine_and_Serine_Metabolism',...
    'S_Tyrosine__Tryptophan__and_Phenylalanine_Metabolism',...
    'S_Arginine_and_Proline_Metabolism', 'S_Cysteine_Metabolism',...
    'S_Methionine_Metabolism', 'S_Valine__Leucine__and_Isoleucine_Metabolism',...
    'S_Other_Amino_Acid_Metabolism', 'S_Asparagine_metabolism', 'S_Nitrogen_Metabolism'};
cofactorsPathsList = {'S_NAD_Biosynthesis', 'S_Pantothenate_and_CoA_Biosynthesis',...
    'S_Folate_Metabolism', 'S_Riboflavin_Metabolism', 'S_Quinone_Biosynthesis',...
    'S_Porphyrin_and_Chlorophyll_Metabolism'};
lipidsPathsList = {'S_Fatty_Acid__Biosynthesis', 'S_Fatty_Acid_Degradation',...
    'S_Fatty_Acid_Metabolism', 'S_Glycerolipid_Metabolism', 'S_Glycoprotein_Metabolism',...
    'S_Phospholipid_Biosynthesis', 'S_Phospholipid_Metabolism',...
    'S_Sphingolipid_Metabolism', 'S_Sterol_Metabolism'};
nucleotidsPathsList = {'S_Purine_and_Pyrimidine_Biosynthesis',...
    'S_Nucleotide_Salvage_Pathway', 'S_Thiamine_Metabolism', 'S_Pyridoxine_Metabolism'};
enrgyPathsList = {'Biomass and maintenance functions', 'S_Anaplerotic_reactions',...
    'S_Oxidative_Phosphorylation'};
colors = createColorsMap;

if isempty(rxnLineColor)||rxnLineColor == "Pathways"
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_GlycolysisGluconeogenesis', 'DARKBLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Pentose_Phosphate_Pathway', 'STEELBLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Pyruvate_Metabolism', 'DODGERBLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Citric_Acid_Cycle', 'MEDIUMBLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Taurine_Metabolism', 'MEDIUMBLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Starch_and_Sucrose_Metabolism', 'BLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Xylose_Metabolism', 'STEELBLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Fructose_and_Mannose_Metabolism', 'DODGERBLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Galactose_metabolism', 'MEDIUMBLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Arabinose_Metabolism', 'BLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Alternate_Carbon_Metabolism', 'DARKBLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Methane_Metabolism', 'DARKBLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Taurine_Metabolism', 'DARKBLUE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Anaplerotic_reactions', 'CRIMSON');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Complex_Alcohol_Metabolism', 'MAROON');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Oxidative_Phosphorylation', 'RED');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Purine_and_Pyrimidine_Biosynthesis', 'PINK');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Nucleotide_Salvage_Pathway', 'HOTPINK');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Thiamine_Metabolism', 'MAGENTA');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Pyridoxine_Metabolism', 'PINK');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_NAD_Biosynthesis', 'PLUM');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Pantothenate_and_CoA_Biosynthesis', 'VIOLET');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Folate_Metabolism', 'DARKORCHID');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Riboflavin_Metabolism', 'PURPLE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Quinone_Biosynthesis', 'MEDIUMORCHID');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Porphyrin_and_Chlorophyll_Metabolism', 'PLUM');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Glutamate_metabolism', 'ORANGE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Glutamine_Metabolism', 'DARKORANGE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Alanine_and_Aspartate_Metabolism', 'CORAL');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Histidine_Metabolism', 'GOLD');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Threonine_and_Lysine_Metabolism', 'ORANGE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Glycine_and_Serine_Metabolism', 'DARKORANGE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Tyrosine__Tryptophan__and_Phenylalanine_Metabolism', 'CORAL');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Arginine_and_Proline_Metabolism', 'GOLD');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Cysteine_Metabolism', 'ORANGE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Methionine_Metabolism', 'DARKORANGE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Valine__Leucine__and_Isoleucine_Metabolism', 'CORAL');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Other_Amino_Acid_Metabolism', 'GOLD');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Asparagine_metabolism', 'ORANGE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Nitrogen_Metabolism', 'DARKORANGE');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Fatty_Acid__Biosynthesis', 'FORESTGREEN');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Fatty_Acid_Degradation', 'MEDIUMSEAGREEN');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Fatty_Acid_Metabolism', 'DARKGREEN');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Glycerolipid_Metabolism', 'SEAGREEN');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Glycoprotein_Metabolism', 'FORESTGREEN');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Phospholipid_Biosynthesis', 'MEDIUMSEAGREEN');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Phospholipid_Metabolism', 'DARKGREEN');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Sphingolipid_Metabolism', 'SEAGREEN');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Sterol_Metabolism', 'FORESTGREEN');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Other', 'BLACK');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'Biomass and maintenance functions', 'RED');
elseif rxnLineColor == "Subsystems"
    modifiedMap = colorSubsystem(modifiedMap, model2, centralPathsList, 'BLUE'); %mapMap to fluxMap
    modifiedMap = colorSubsystem(modifiedMap, model2, aminoacPathsList, 'ORANGE');
    modifiedMap = colorSubsystem(modifiedMap, model2, cofactorsPathsList, 'ORCHID');
    modifiedMap = colorSubsystem(modifiedMap, model2, lipidsPathsList, 'LIME');
    modifiedMap = colorSubsystem(modifiedMap, model2, nucleotidsPathsList, 'MAGENTA');
    modifiedMap = colorSubsystem(modifiedMap, model2, enrgyPathsList, 'RED');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Complex_Alcohol_Metabolism', 'GOLDENROD');
    modifiedMap = colorSubsystem(modifiedMap, model2, 'S_Other', 'GOLDENROD');
elseif ~(rxnLineColor == "Subsystems" || rxnLineColor == "Payhways" || isempty(rxnLineColor))
    for i = 1:length(modifiedMap.rxnName)
        a = find(ismember(model2.rxns, modifiedMap.rxnName{i}));
        if ~isempty(a) & string(model2.subSystems(a)) ~= "NonIncluded"
            modifiedMap.rxnColor(i, 1) = {colors(rxnLineColor)};
        end
    end
end
if rxnLineColor == "Subsystems" || rxnLineColor == "Payhways" || isempty(rxnLineColor)
    h2o = {'H2Ot', 'H2Otm',	'H2Otn', 'H2Otv', 'H2Otp', 'H2Oter'};
    for i = 1:length(modifiedMap.rxnName)
        a = find(ismember(model2.rxns, modifiedMap.rxnName{i}));
        for j = 1:length(h2o)
            if ~isempty(a) & string(modifiedMap.rxnName{i}) == string(h2o(j))...
                    & string(model2.subSystems(a)) ~= "NonIncluded"
                modifiedMap.rxnColor(i, 1) = {colors('CYAN')};
            end
        end
    end
    o2 = {'O2t', 'O2tm', 'O2ter', 'O2tp'};
    for i = 1:length(modifiedMap.rxnName)
        a = find(ismember(model2.rxns, modifiedMap.rxnName{i}));
        for j = 1:length(o2)
            if ~isempty(a) & string(modifiedMap.rxnName{i}) == string(o2(j))...
                    & string(model2.subSystems(a)) ~= "NonIncluded"
                modifiedMap.rxnColor(i, 1) = {colors('CYAN')};
            end
        end
    end
    Pi = {'PIt2r', 'PIt2m', 'PIt5m', 'PIt2n', 'PIt2v', 'PIt2p', 'ATPtm_H', 'ATPtp_H', ...
        'ATP2tp_H', 'ATPS3g', 'ATPS3v', 'UTPtm', 'CTPtm', 'GTPt2m', 'ATPS'};
    for i = 1:length(modifiedMap.rxnName)
        a = find(ismember(model2.rxns, modifiedMap.rxnName{i}));
        for j = 1:length(Pi)
            if ~isempty(a) & string(modifiedMap.rxnName{i}) == string(Pi(j))...
                    & string(model2.subSystems(a)) ~= "NonIncluded"
                modifiedMap.rxnColor(i, 1) = {colors('RED')};
            end
        end
    end
    co2 = {'CO2t', 'CO2tg', 'CO2tm', 'CO2tn', 'HCO3tn', 'CO2tv', 'CO2tp'};
    for i = 1:length(modifiedMap.rxnName)
        a = find(ismember(model2.rxns, modifiedMap.rxnName{i}));
        for j = 1:length(co2)
            if ~isempty(a) & string(modifiedMap.rxnName{i}) == string(co2(j))...
                    & string(model2.subSystems(a)) ~= "NonIncluded"
                modifiedMap.rxnColor(i, 1) = {colors('SLATEGRAY')};
            end
        end
    end
    aa = {'MMETt2', 'GLUt2r', 'GLNt2r', 'GLNt7', 'GLUt7', 'LCYSTintv', 'NH4t', 'TCHOLAabcv'};
    for i = 1:length(modifiedMap.rxnName)
        a = find(ismember(model2.rxns, modifiedMap.rxnName{i}));
        for j = 1:length(aa)
            if ~isempty(a) & string(modifiedMap.rxnName{i}) == string(aa(j))...
                    & string(model2.subSystems(a)) ~= "NonIncluded"
                modifiedMap.rxnColor(i, 1) = {colors('ORANGE')};
            end
        end
    end
    cenMet = {'COAtim', 'NAt3_1', 'MALS'};
    for i = 1:length(modifiedMap.rxnName)
        a = find(ismember(model2.rxns, modifiedMap.rxnName{i}));
        for j = 1:length(cenMet)
            if ~isempty(a) & string(modifiedMap.rxnName{i}) == string(cenMet(j))...
                    & string(model2.subSystems(a)) ~= "NonIncluded"
                modifiedMap.rxnColor(i, 1) = {colors('BLUE')};
            end
        end
    end
    lip = {'G3PIt'};
    for i = 1:length(modifiedMap.rxnName)
        a = find(ismember(model2.rxns, modifiedMap.rxnName{i}));
        for j = 1:length(lip)
            if ~isempty(a) & string(modifiedMap.rxnName{i}) == string(lip(j))...
                    & string(model2.subSystems(a)) ~= "NonIncluded"
                modifiedMap.rxnColor(i, 1) = {colors('LIME')};
            end
        end
    end
end

if rxnsFluxTask == 5 % change color to dark-color in negative values
    for i = 1:length(modifiedMap.rxnName)
        valueskeys = values(colors)';
        valueskeys (:,2) = keys(colors)';
        a = find(ismember(valueskeys, modifiedMap.rxnColor{i, 1}));
        b = find(ismember(model2.rxns, modifiedMap.rxnName{i}));
        if ~isempty(b) & v1(b) < 0 & string(model2.subSystems(b)) ~= "NonIncluded"
            if string(valueskeys (a,2)) == 'LIME'
                modifiedMap.rxnColor(i, 1) = {colors('GREEN')};
            else
                rxnColor =strcat('DARK', char(valueskeys (a(1,1),2)));
                if string(rxnColor) == 'DARKAQUA'
                    rxnColor = 'DARKCYAN';
                elseif string(rxnColor) == 'DARKFUCHSIA'
                    rxnColor = 'DARKMAGENTA';
                end
                modifiedMap.rxnColor(i, 1) = {colors(rxnColor)};
            end
        end
    end
end

end


%% CONTRAST2_FSU uses the fsuClusterMatlab command to run the CONTRAST2 computation.
%
%  Discussion:
%
%    The arguments to fsuClusterMatlab mean:
%
%    [] allows us to specify an output directory;
%    [] allows us to specify switches to the MOAB queue handler, such as more time.
%    'm" indicates this is an "MPI-like" job;
%    'w' indicates that the MATLAB session should wait for completion;
%    4 is the number of processors requested;
%    @contrast2_fun indicates the function we want to execute;
%    {} would contain input arguments if there were any.
%
%    The function "contrast2_fun" must correspond to a MATLAB M-file "contrast2_fun.m"
%    and that file must be in MATLAB's path.
%
%    The easiest way to ensure that the file is in MATLAB's path is to
%    create a subdirectory called "matlab" immediately below your home directory,
%    and place the function file there (as well as any other files needed by the job).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2010
%
%  Author:
%
%    John Burkardt
%

%
%  Read a black and white image.
%
  x = imread ( 'surfsup.tif' );

  fsuClusterMatlab ( [], [], 'm', 'w', 4, @contrast2_fun, { x } )


function T2 = shrink_obs_dims_in_table(T1, dom, evidence)
% SHRINK_OBS_DIMS_IN_TABLE Set observed dimensions to size 1
% T2 = shrink_obs_dims_in_table(T1, dom, evidence)
%
% If 'T1' contains observed nodes, it will have 0s in the positions that are
% inconsistent with the evidence. We now remove these 0s and set the corresponding dimensions to
% size 1, to be consistent with the way most inference engines handle evidence, which is to
% shrink observed nodes before doing inference.

% This is used by pearl and enumerative inf. engines.

odom = dom(~isemptycell(evidence(dom)));
vals = cat(1,evidence{odom});
ndx = mk_multi_index(length(dom), find_equiv_posns(odom, dom), vals(:));
T2 = T1(ndx{:});


function nlp  = LoadProblem(system, bounds, gait, load_path)

    if nargin < 4
        load_path = [];
    end

    num_grid.RightStance1 = 10;
    num_grid.LeftStance1 = 10;
    num_grid.RightStance2 = 10;
    num_grid.LeftStance2 = 10;

    options = {'EqualityConstraintBoundary', 1e-4,...
        'DistributeTimeVariable', false,...
        'DistributeParameters',false};

    nlp = HybridTrajectoryOptimization('atrias_opt', system, num_grid, [], options{:});

    if isempty(load_path)
        nlp.configure(bounds);
    else
        nlp.configure(bounds, 'LoadPath',load_path);
    end
    % cost function
    %     opt.cost.Power(nlp, system);
    trans_opt.cost.StateDeviation(nlp, system, gait);
    nlp.update;

end


%DstarPO D*-PO navigation class
%
% A concrete subclass of the Navigation class that implements the D*
% navigation algorithm; facilitates incremental replanning. This
% implementation of D* is intended for multiobjective optimization (MOO)
% problems - i.e. optimizes over several objectives/criteria - with the use
% of Pareto fronts (see Lavin paper).
%
% Methods::
% plan              Compute the cost map given a goal and map
% path              Compute a path to the goal
% visualize         Display the obstacle map (deprecated)
% plot              Display the obstacle map
% cost_get          Return the specified cost layer
% costmap_modify    Modify the costmap
% modify_cost       Modify the costmap (deprecated, use costmap_modify)
% costmap_get       Return the current costmap
% costmap_set       Set the current costmap
% distancemap_get   Set the current distance map
% display           Print the parameters in human readable form
% char              Convert to string
%
% Properties::
% TBD
%
% Example::
%        load map1              % load map
%        goal = [50,30];
%        start=[20,10];
%        ds = DstarPO(map);     % create navigation object
%        ds.plan(goal,1)        % create plan for specified goal
%        ds.path(start)         % animate path from this start location
% Example 2:
%       goal = [100;100];
%       start = [1;1];
%       ds = DstarPO(0);        % create Navigation object with random occupancy grid
%       ds.addCost(1,L);        % add 1st add'l cost layer L
%       ds.plan(goal,2);        % setup costmap for specified goal
%       ds.path(start);         % plan solution path start-goal, animate
%       P = as.path(start);     % plan solution path start-goal, return path
%
% Notes::
% - Obstacles are represented by Inf in the costmap.
%
% References::
% - The D* algorithm for real-time planning of optimal traverses,
%   A. Stentz, Tech. Rep. CMU-RI-TR-94-37, The Robotics Institute,
%   Carnegie-Mellon University, 1994.
% - A Pareto Optimal D* Search Algorithm for Multiobjective Path Planning,
%   A. Lavin.
% - Robotics, Vision & Control, Sec 5.2.2,
%   Peter Corke, Springer, 2011.
%
% Author::
% Alexander Lavin based on Dstar by Peter Corke
%
% See also Navigation, Dstar, DstarMOO, Astar, DXform.

% Copyright (C) 1993-2015, by Peter I. Corke, Alexander Lavin
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB 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.
%
% RTB 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 RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
%
% This is an original algorithm written by Alexander Lavin.
% http://alexanderlavin.com

% Implementation notes:
%
% All the state is kept in the structure called d
% X is an index into the array of states.
% state pointers are kept as matlab array index rather than row,col format

classdef DstarPO < Navigation

    properties (SetAccess=private, GetAccess=private)

        costmap   % world cost map: obstacle = Inf
        G         % index of goal point
        N         % number of objectives

        % info kept per cell (state)
        b       % backpointer (0 means not set)
        t       % tag: NEW OPEN CLOSED

        cost_g       % path distance summation
        cost_h       % path heuristic (state to goal) cost
        cost_01      % add'l cost layer 01 (unused)
        cost_02      % add'l cost layer 02 (unused)
        cost_03      % add'l cost layer 03 (unused)
                     % add more cost layers if needed...

        priority
        tie

        % list of open states: 2xN matrix
        %   each open point is a column, row 1 = index of cell, row 2 = k
        openlist
        niter

        changed

        openlist_maxlen     % keep track of maximum length
        quiet

        % tag state values
        NEW = 0;
        OPEN = 1;
        CLOSED = 2;
    end

    methods

        % constructor
        function ds = DstarPO(world, varargin)
            %DstarPO.DstarPO D*-PO constructor
            %
            % DS = Dstar(MAP, OPTIONS) is a D* navigation object, and MAP is an
            % occupancy grid, a representation of a planar world as a
            % matrix whose elements are 0 (free space) or 1 (occupied).
            % The occupancy grid is coverted to a costmap with a unit cost
            % for traversing a cell.
            %
            % Options::
            % 'goal',G      Specify the goal point (2x1)
            % 'metric',M    Specify the distance metric as 'euclidean' (default)
            %               or 'cityblock'.
            % 'inflate',K   Inflate all obstacles by K cells.
            % 'quiet'       Don't display the progress spinner
            %
            % Other options are supported by the Navigation superclass.
            %
            % Notes::
            % - If MAP == 0 a random map is created.
            %
            % See also Navigation.Navigation.

            % invoke the superclass constructor
            ds = ds@Navigation(world, varargin{:});

            opt.quiet = false;
            opt = tb_optparse(opt, varargin);
            ds.quiet = opt.quiet;

            ds.occgrid2costmap(ds.occgrid);

            % init the D* state variables
            ds.reset();
            if ~isempty(ds.goal)
                ds.goal_change();
            end
            ds.changed = false;
        end

        function reset(ds)
            %DstarPO.reset Reset the planner
            %
            % DS.reset() resets the D* planner.  The next instantiation
            % of DS.plan() will perform a global replan.

            % build the matrices required to hold the state of each cell for D*
            ds.b = zeros(size(ds.costmap), 'uint32');     % backpointers
            ds.t = zeros(size(ds.costmap), 'uint8');      % tags
            ds.cost_g = Inf*ones(size(ds.costmap));       % path cost estimate
            ds.openlist = zeros(2,0);                     % the open list, one column per point

            ds.openlist_maxlen = -Inf;
        end

        function goal_change(ds)

            if isempty(ds.b)
                return;
            end
            goal = ds.goal;

            % keep goal in index rather than row,col format
            ds.G = sub2ind(size(ds.occgrid), goal(2), goal(1));
            ds.INSERT(ds.G, ds.projectCost(ds.G), 'goalset');
            ds.cost_g(ds.G) = 0;

            % new goal changes cost layers:
            ds.calcHeuristic(ds.occgrid, ds.goal);
        end

        function s = char(ds)
            %DstarPO.char Convert navigation object to string
            %
            % DS.char() is a string representing the state of the Dstar
            % object in human-readable form.
            %
            % See also Dstar.display, Navigation.char.

            % most of the work is done by the superclass
            s = char@Navigation(ds);

            % Dstar specific stuff
            if ~isempty(ds.costmap)
                s = char(s, sprintf('  costmap: %dx%d, open list %d', size(ds.costmap), numcols(ds.openlist)));
            else
                s = char(s, sprintf('  costmap: empty:'));
            end
        end

        function plot(ds, varargin)
            %DstarPO.plot Visualize navigation environment
            %
            % DS.plot() displays the occupancy grid and the goal distance
            % in a new figure.  The goal distance is shown by intensity which
            % increases with distance from the goal.  Obstacles are overlaid
            % and shown in red.
            %
            % DS.plot(P) as above but also overlays a path given by the set
            % of points P (Mx2).
            %
            % See also Navigation.plot.

            plot@Navigation(ds, 'distance', ds.cost_h, varargin{:});
        end

        % invoked by Navigation.step
        function n = next(ds, current)

            if ds.changed
                error('Cost map has changed, replan');
            end
            X = sub2ind(size(ds.costmap), current(2), current(1));
            X = ds.b(X);
            if X == 0
                n = [];
            else
                [r,c] = ind2sub(size(ds.costmap), X);
                n = [c;r];
            end
        end

        function plan(ds, goal, N)
            %DstarPO.plan Plan path to goal
            %
            % DS.plan() updates DS with a costmap of distance to the
            % goal from every non-obstacle point in the map.  The goal is
            % as specified to the constructor.
            %
            % DS.plan(GOAL) as above but uses the specified goal.
            %
            % Note::
            % - If a path has already been planned, but the costmap was
            %   modified, then reinvoking this method will replan,
            %   incrementally updating the plan at lower cost than a full
            %   replan.
            %
            % Inputs:
            %   goal: goal state coordinates
            %   N: number of optimization objectives; standard D* is 2
            %   (i.e. distance and heuristic)

            ds.N = N; % number of optimization objectives
            ds.openlist = zeros(ds.N+1,0);

            % Setup cost layers. If a
            % cost layer is goal-dependent, it's setup function needs to
            % also be called in DS.goal_change(). If more cost layers are
            % needed, add similar to DS.cost_01.

            % initializations first:
            ds.cost_g = zeros(size(ds.occgrid));
            ds.cost_h = zeros(size(ds.occgrid)); % filled after setting goal below
            % if add'l costs haven't been added with addCost()
            if isempty(ds.cost_01)
                ds.cost_01 = zeros(size(ds.occgrid));
            end
            if isempty(ds.cost_02)
                ds.cost_02 = zeros(size(ds.occgrid));
            end
            if isempty(ds.cost_03)
                ds.cost_03 = zeros(size(ds.occgrid));
            end

            if nargin > 1
                ds.goal = goal; % invokes superclass method set.goal()
            end
            % for replanning no goal is needed,
            if isempty(ds.goal)
                error('must specify a goal point');
            end

            % Setup cost layers DS.cost_g and DS.cost_h.
            % assign values to the distance cost layer, set as DS.costmap
            ds.occgrid2costmap(ds.occgrid);
            % assign values to the heuristic cost layer, set as DS.cost_h
            ds.calcHeuristic(ds.occgrid, ds.goal);
            % Additional cost layers are added by the user with the
            % DS.addCost() method

            % Cost priority/tiebreaker: cost_g (distance to node)
            ds.priority = ds.cost_g;
            ds.tie = 1; % first cost: cost_g

            ds.niter = 0;
            while true
                if ~ds.quiet && mod(ds.niter, 20) == 0
                    ds.spinner();
                end
                ds.niter = ds.niter + 1;

                if ds.PROCESS_STATE() < 0
                    break;
                end
                if ds.verbose
                    disp(' ')
                end
            end
            if ~ds.quiet
                fprintf('\r');
            end
            ds.changed = false;
        end

        function layer = cost_get(ds)
        %DstarPO.cost_get Get the specified cost layer
            layer = ds.cost_02;
        end

        function c = distancemap_get(ds)
        %DstarPO.distancemap_get Get the current distance map
        %
        % C = DS.distancemap_get() is the current distance map.  This map is the same size
        % as the occupancy grid and the value of each element is the shortest distance
        % from the corresponding point in the map to the current goal.  It is computed
        % by Dstar.plan.
        %
        % See also Dstar.plan.
            c = ds.cost_h; % heuristic
        end

        function c = costmap_get(ds)
        %DstarPO.costmap_get Get the current costmap
        %
        % C = DS.costmap_get() is the current costmap.  The cost map is the same size
        % as the occupancy grid and the value of each element represents the cost
        % of traversing the cell.  It is autogenerated by the class constructor from
        % the occupancy grid such that:
        % - free cell (occupancy 0) has a cost of 1
        % - occupied cell (occupancy >0) has a cost of Inf
        %
        % See also Dstar.costmap_set, Dstar.costmap_modify.

            c = ds.costmap;
        end

        function costmap_set(ds, costmap)
        %DstarPO.costmap_set Set the current costmap
        %
        % DS.costmap_set(C) sets the current costmap.  The cost map is the same size
        % as the occupancy grid and the value of each element represents the cost
        % of traversing the cell.  A high value indicates that the cell is more costly
        % (difficult) to traverese.  A value of Inf indicates an obstacle.
        %
        % Notes::
        % - After the cost map is changed the path should be replanned by
        %   calling DS.plan().
        %
        % See also Dstar.costmap_get, Dstar.costmap_modify.
            if ~all(size(costmap) == size(ds.occgrid))
                error('costmap must be same size as occupancy grid');
            end
            ds.costmap = costmap;
            ds.changed = true;
        end

        function costmap_modify(ds, point, newcost)
        %DstarPO.costmap_modify Modify cost map
        %
        % DS.costmap_modify(P, NEW) modifies the cost map at P=[X,Y] to
        % have the value NEW.  If P (2xM) and NEW (1xM) then the cost of
        % the points defined by the columns of P are set to the corresponding
        % elements of NEW.
        %
        % Notes::
        % - After one or more point costs have been updated the path
        %   should be replanned by calling DS.plan().
        % - Replaces modify_cost, same syntax.
        %
        % See also Dstar.costmap_set, Dstar.costmap_get.

            if numel(point) == 2
                % for case of single point ensure it is a column vector
                point = point(:);
            end
            if numcols(point) ~= numcols(newcost)
                error('number of columns in point must match columns in newcost');
            end
            for i=1:numcols(point)
                X = sub2ind(size(ds.costmap), point(2,i), point(1,i));
                ds.costmap(X) = newcost(i);
            end
            if ds.t(X) == ds.CLOSED
                ds.INSERT(X, ds.h(X), 'modifycost');
            end
            ds.changed = true;
        end

        function addCost(ds, layer, values)
        %DstarPO.addCost Add an additional cost layer
        %
        % DS.addCost(layer,values) adds the matrix specified by values as a
        % cost layer.
        % Inputs
        %   layer: 1, 2, or 3 to specify which cost layer to add
        %   values: normalized matrix the size of the environment (100x100)
            if size(values)~=size(ds.occgrid)
                display('Layer size does not match the environment')
                return
            end
            if max(max(values))~=1 || min(min(values))~=0
                display('Layer values are not normalized [0:1]')
                return
            end

            if layer==1
                ds.cost_01 = values;
            elseif layer==2
                ds.cost_02 = values;
            elseif layer==3
                ds.cost_03 = values;
            else
                display('Layer index out of range')
            end
        end

    end % public methods

    methods (Access=protected)

        function occgrid2costmap(ds, og, cost)
            if nargin < 3
                cost = 1;
            end
            ds.costmap = og;
            ds.costmap(ds.costmap==1) = Inf;      % occupied cells have Inf driving cost
            ds.costmap(ds.costmap==0) = cost;     % unoccupied cells have driving cost
        end

        function calcHeuristic(ds, grid, goal)
            ds.cost_h=zeros(size(grid));
            for ii=1:size(grid,1)
                for jj=1:size(grid,2)
                    ds.cost_h(ii,jj)=sqrt((ii-goal(1))^2+(jj-goal(2))^2);
                end
            end
        end

        % The main D* function as per the Stentz paper, comments Ln are the original
        % line numbers.
        function r = PROCESS_STATE(d)
            % States with the lowest cost value are removed from the
            % open list

            % Get Pareto optimal point off the open list
            [idx] = paretofront(normc(d.openlist(2:size(d.openlist,1),:)')); % w/ normalization
            front = d.openlist(:,idx);
            [k_old,col] = min(front(d.tie+1,:)); % d.tie specifies the row (cost_g) for tiebreaker
            X = front(1,col); % L1

            if isempty(X) % L2
                r = -1;
                return;
            end

            d.DELETE(X); % L3

            d.priority = d.cost_g; % updates priority cost layer

            if k_old < d.priority(X) % L4
                d.message('k_old < h(X):  %f %f\n', k_old, d.priority(X));
                for Y=d.neighbours(X) % L5
                    if (d.priority(Y) <= k_old) && (d.priority(X) > d.updateCosts(Y,X,0))  % L6
                        d.b(X) = Y;
                        d.updateCosts(X,Y,d.N); %L7
                    end
                end
            end

            % can we lower the path cost of any neighbours?
            if k_old == d.priority(X) % L8
                d.message('k_old == h(X): %f\n', k_old);
                for Y=d.neighbours(X) % L9
                    if (d.t(Y) == d.NEW) || ... % L10-12
                            ( (d.b(Y) == X) && (d.priority(Y) ~= d.updateCosts(Y,X,0)) ) || ...
                            ( (d.b(Y) ~= X) && (d.priority(Y) > d.updateCosts(Y,X,0)) )
                        % Update and project the costs:
                        d.updateCosts(Y,X,d.N);
                        objspace = d.projectCost(Y,X);
                        d.b(Y) = X;
                        d.INSERT(Y, objspace, 'L13'); % L13
                    end
                 end
            else % L14
                d.message('k_old > h(X)');
                for Y=d.neighbours(X) % L15
                    if (d.t(Y) == d.NEW) || ( (d.b(Y) == X) && (d.priority(Y) ~= d.updateCosts(Y,X,0)) )
                        d.updateCosts(Y,X,d.N);
                        objspace = d.projectCost(Y,X);
                        d.b(Y) = X;
                        d.INSERT(Y, objspace, 'L18'); % L18
                    else
                        if ( (d.b(Y) ~= X) && (d.priority(Y) > d.updateCosts(Y,X,0)) )
                            d.INSERT(X, d.projectCost(X), 'L21');                    % L21
                        else
                            if (d.b(Y) ~= X) && (d.priority(X) > d.updateCosts(Y,X,0)) && ...
                                    (d.t(Y) == d.CLOSED) && d.priority(Y) > k_old
                                d.INSERT(Y, d.projectCost(Y), 'L25');                % L25
                            end
                        end
                    end
                 end
            end

            r = 0;
            return;
        end % process_state

        function k_new = updateCosts(ds, a, b, obj)
            % NOTE: Only for costs that accumulate (i.e. sum) over the
            % path, and for dynamic costs.
            % E.g. the heuristic parameter DS.cost_h only needs updating
            % when the goal state changes; it's values are stored for each
            % cell.
            %
            % Location moving from state b to a.
            if nargout > 0
                k_new = ds.cost_g(b) + ds.dc(b,a);
                return
            end
            if obj == 0                        % just return what the new priority cost would be (k_new)
                return
            end
            if obj > 1                         % base case
                ds.cost_g(a) = ds.cost_g(b) + ds.dc(b,a);
                % (no heuristic update needed)
            end
            if obj > 2                         % w/ cost_01: elevation
                % (no elevation update needed)
            end
            if obj > 3                         % w/ cost_02: solar
                sV = [cos(ds.niter/100);sin(ds.niter/100)]; % rotates 1rad per 100 steps
                ds.cost_02(a) = dot(sV,ds.vc(b,a));
            end
            if obj > 4                         % w/ cost_03: risk
                % (no risk update needed)
            end
        end

        function pt = projectCost(ds, a, b)
            % Returns the projection of state a into objective space. If
            % specified, location is moving from b to a.
            switch nargin
                case 2
                    pt = [ds.cost_g(a);
                          ds.cost_h(a);
                          ds.cost_01(a);
                          ds.cost_02(a);
                          ds.cost_03(a);
                          ];
                case 3
                    pt = [ds.cost_g(b) + ds.dc(a,b);
                          ds.cost_h(a);
                          ds.cost_01(a);
                          ds.cost_02(a);
                          ds.cost_03(a);
                          ];
                otherwise
                    return
            end
        end

        function kk = k(ds, X)
            i = ds.openlist(1,:) == X;
            kk = ds.openlist(2, i);
        end

        function INSERT(ds, X, pt, where)
            % Add state X to the openlist with objective space values
            % specified by pt.

            % where is for diagnostic purposes only
            ds.message('insert (%s) %d = %f\n', where, X, pt);

            i = find(ds.openlist(1,:) == X);
            if length(i) > 1
                error('D*:INSERT: state in open list %d times', X);
            end

            if ds.t(X) == ds.NEW
                % add a new column to the open list
                ds.openlist = [ds.openlist [X; pt]];
            elseif ds.t(X) == ds.OPEN
%                 k_new = min( ds.openlist(2,i), h_new );
            elseif ds.t(X) == ds.CLOSED
                if pt(ds.tie) < ds.priority(X) % break tie w/ ds.tie index
                    % add a new column to the open list
                    ds.openlist = [ds.openlist [X; pt]];
                end
            end

            % keep track of the max length of the openlist:
            if numcols(ds.openlist) > ds.openlist_maxlen
                ds.openlist_maxlen = numcols(ds.openlist);
            end

            ds.t(X) = ds.OPEN;
        end

        function DELETE(ds, X)
            ds.message('delete %d\n', X);
            i = find(ds.openlist(1,:) == X);
            if length(i) ~= 1
                error('D*:DELETE: state %d doesnt exist', X);
            end
            ds.openlist(:,i) = []; % remove the column
            ds.t(X) = ds.CLOSED;
        end

        function kmin = GET_KMIN(ds)
            kmin = min(ds.openlist(2,:));
        end

        % return the distance of moving from state X to state Y
        function cost = dc(ds, X, Y)
            [r,c] = ind2sub(size(ds.costmap), [X; Y]);
            dist = sqrt(sum(diff([r c]).^2));
            dcost = (ds.costmap(X) + ds.costmap(Y))/2;

            cost = dist * dcost;
        end

        % return the robot unit vector; direction of moving from state X to state Y
        function vector = vc(ds, X, Y)
            [Xi,Xj] = ind2sub(size(ds.occgrid),X);
            [Yi,Yj] = ind2sub(size(ds.occgrid),Y);
            vector = [Yi-Xi;Yj-Xj];
            vector = vector/norm(vector);
%             slope = vector(2) / vector(1);
%             theta = dot([0,1],[vector])/(norm([0,1])*norm(vector));
        end

        % return index of neighbour states as a row vector
        function Y = neighbours(ds, X)
            dims = size(ds.costmap);
            [r,c] = ind2sub(dims, X);

            % list of 8-way neighbours
            Y = [r-1 r-1 r-1 r r  r+1 r+1 r+1; c-1 c c+1 c-1 c+1 c-1 c c+1];
            % only use neighbors w/in grid bounds...
            k = (min(Y)>0) & (Y(1,:)<=dims(1)) & (Y(2,:)<=dims(2));
            Y = Y(:,k);
            Y = sub2ind(dims, Y(1,:)', Y(2,:)')';
        end

    end % end of protected methods
end % classdef


function allw = traceToWaveform(obj)
   %traceToWaveform converts traceobjects to waveforms
   %    w = traceToWaveform(antelopesource)
   %
   % Note: this may return multiple waveform objects, depending upon
   % how many segments and/or scnl's.

   wBlank = antelopesource.blankWaveformWithCalib();

   try
      traceCount = dbnrecs(obj.trpointer);
      assert(traceCount > 0);
   catch er
      disp('traceToWaveform: No traces found to convert');
      allw = wBlank([]);
      return
   end

   % preallocations
   spikes(traceCount).mask = false; %used to track data spikes & inf values
   allw = repmat(wBlank,traceCount,1);

   % LOOP twice through segments represented by this trace object
   % 1st loop: find signal spikes, and get header info.
   for seg = 1:traceCount
      obj.trpointer.record = seg - 1;
      allw(seg) = waveformMetadata(wBlank, obj.trpointer);
      spikes(seg).mask = findSpikes(obj.trpointer);
   end

   allw = applyCalibrationsToAll(trpointer, allw);

   % 2nd loop: assign data to the waveforms.
   replaceWithNan = @(W,BAD) setsamples(W, BAD.mask, nan);
   for seg = 1:traceCount
      obj.trpointer.record = seg - 1;
      allw(seg) = set(allw(seg), 'data', trextract_data(obj.trpointer));
      allw(seg) = replaceWithNan(allw(seg), spikes(seg));
   end
end

function w = waveformMetadata(w, trpointer)
   s = db2struct(trpointer); %do once, get one for each segment
   [sunit, ~] = segtype2units(s.segtype);
   w = set(w, ...
      'station', s.sta, ...
      'channel', s.chan, ...
      'network', s.net, ...
      ...'location', s.loc, ... % unknown if 'loc' really is a field
      'start', datenum(epoch2str(s.time,'%Y-%m-%d %H:%M:%S.%s')), ...
      'freq', s.samprate, ...
      'units', sunit, ...
      'calib', s.calib);
end

function spikes = findSpikes(trpointer)
      % data spikes must be known PRIOR to applying calibration
      maxAllowableSignal = (realmax('single') * 1e-2);
      data = trextract_data(trpointer);
      spikes = (abs(data) >= maxAllowableSignal) | isinf(data);
end

function allw = applyCalibrationsToAll(trpointer, allw)
   % now, apply calibrations to all traces at once
   trapply_calib(trpointer);
   hasCalibs = get(allw,'calib') ~= 0;
   allw(hasCalibs) = set(allw(hasCalibs),'calibration_applied','YES');
end


function cv = combine_visitors(varargin)
% COMBINE_VISITORS Generate a new visitor by combining existing visitors
%
% cv = combine_visitors(v1, v2, ...) generates a new algorithm visitor that
% works by calling the functions in v1 followed by the functions in v2, and
% so on.
%
% The value returned by the combined visitor function is the bitwise & of
% all the individual return values.  So, if any visitor requests the
% algorithm to halt, then the algorithm will halt.
%
% Note: using a combined visitor is somewhat slower than writing a custom
% combined visitor yourself.
%
% Example:
%    vis1 = struct();
%    vis1.examine_vertex = @(u) fprintf('vis1: examine_vertex(%i)\n', u);
%    vis2 = struct();
%    vis2.examine_vertex = @(u) fprintf('vis2: examine_vertex(%i)\n', u);
%    combined_vis = combine_visitors(vis1, vis2);
%    load graphs/bfs_example.mat
%    breadth_first_search(A,1,combined_vis);

% David Gleich
% Copyright, Stanford University, 2006-2008

% trivial output
if isempty(varargin)
    error('matlab_bgl:invalidParameter', 'combine_visitors requires at least one argument');
end

if length(varargin) == 1, cv = varargin{1}; return; end

cv_fn = struct();

for ii = 1:length(varargin)
    fn = fieldnames(varargin{ii});

    for jj = 1:length(fn)
        if ~isfield(cv_fn,fn{jj})
            cv_fn.(fn{jj}) = 1;
        else
            cv_fn.(fn{jj}) = cv_fn.(fn{jj}) + 1;
        end
    end
end

    function cfunc=combine_function(name,count)

        used_visitors = cell(count,1);
        cur_used = 1;

        for kk = 1:length(varargin)
            if isfield(varargin{kk}, name)
                used_visitors{cur_used} = varargin{kk}.(name);
                cur_used = cur_used + 1;
            end
        end

        function rval = combine_function_impl(varargin)
            rval = 1;
            for ll=1:length(used_visitors)
                try
                    stop = feval(used_visitors{ll}, varargin{:});
                    rval = rval & double(stop);
                catch
                    [lastmsg,lastid] = lasterr;
                    if ~strcmp(lastid, 'MATLAB:TooManyOutputs')
                        rethrow(lasterr);
                    else
                        feval(used_visitors{ll}, varargin{:});
                    end
                end
            end
        end

        cfunc = @combine_function_impl;

    end

cv = struct();

% specify only the visitors functions that only occured once
for ii = 1:length(varargin)
    fn = fieldnames(varargin{ii});

    for jj = 1:length(fn)
        if (cv_fn.(fn{jj}) == 1)
            cv.(fn{jj}) = varargin{ii}.(fn{jj});
        end
    end
end

% specify all visitors that occured more than once
fn = fieldnames(cv_fn);
for jj=1:length(fn)
    if (cv_fn.(fn{jj}) > 1)
        cv.(fn{jj}) = combine_function(fn{jj}, cv_fn.(fn{jj}));
    end
end

% overall function end
end


function y = cvx_isvalid( x, full )
y = isnan( x ) | isinf( x );
if nargin < 2 || ~full,
	y = ~any( y(:) );
else
	y = ~y;
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 [varargout] = ismatrix(varargin)

% ISMATRIX is a drop-in replacement for the same function that was
% introduced in MATLAB R2010b.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% see https://github.com/fieldtrip/fieldtrip/issues/899

alternatives = which(mfilename, '-all');
if ~iscell(alternatives)
  % this is needed for octave, see https://github.com/fieldtrip/fieldtrip/pull/1171
  alternatives = {alternatives};
end

keep = true(size(alternatives));
for i=1:numel(alternatives)
  keep(i) = keep(i) && ~any(alternatives{i}=='@');  % exclude methods from classes
  keep(i) = keep(i) && alternatives{i}(end)~='p';   % exclude precompiled files
end
alternatives = alternatives(keep);

if exist(mfilename, 'builtin') || any(strncmp(alternatives, matlabroot, length(matlabroot)) & cellfun(@isempty, strfind(alternatives, fullfile('private', mfilename))))
  % remove this directory from the path
  p = fileparts(mfilename('fullpath'));
  sw = warning('off','backtrace');
  warning('Removing "%s" from your path.\nSee http://www.fieldtriptoolbox.org/faq/should_i_add_fieldtrip_with_all_subdirectories_to_my_matlab_path/', p);
  warning(sw);
  rmpath(p);
  % call the original MATLAB function
  if exist(mfilename, 'builtin')
    [varargout{1:nargout}] = builtin(mfilename, varargin{:});
  else
    [varargout{1:nargout}] = feval(mfilename, varargin{:});
  end
  return
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% this is where the actual replacement code starts
% function tf = ismatrix(x)

% deal with the input arguments
if nargin==1
  [x] = deal(varargin{1:1});
else
  error('incorrect number of input arguments')
end

siz = size(x);
tf = numel(siz)==2 && siz(1)>=0 && siz(2)>=0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% deal with the output arguments

varargout = {tf};


classdef ODE113Integrator < AbstractFirstOrderIntegrator
    %ODE45Integrator Summary of this class goes here
    %   Detailed explanation goes here

    properties
        options(1,1) BuiltInIntegratorOptions = BuiltInIntegratorOptions();
        integratorEnum = IntegratorEnum.ODE113;
    end

    methods
        function obj = ODE113Integrator(options)
            if(nargin > 0)
                obj.options = options;
            else
                obj.options = BuiltInIntegratorOptions();
            end
        end

        function [t,y,te,ye,ie] = integrate(obj, odefun, tspan, y0, evtsFunc, odeOutputFun)
            odeSetOptions = obj.options.getIntegratorOptions();
            optionsToUse = odeset(odeSetOptions, 'Events',evtsFunc, 'OutputFcn',odeOutputFun);

            [t,y,te,ye,ie] = ode113(odefun, tspan, y0, optionsToUse);
        end

        function options = getOptions(obj)
            options = obj.options;
        end
    end
end


function varargout = ECGBeatFitter(varargin)
%
% ECGBeatFitter(ECG,Phase,ExpParamName),
% Overview: Graphical user interface for ECG approximation with Gaussian kernels.
%
% inputs:
% ECG: a single ECG waveform used for model training
% Phase: the phase corresponding to the ECG waveform
% ExpParamName (optional): The name of the vector containing the exported parameter (default:OptimumParams)
% ExpParamName (optional): The name of the vector containing the exported parameter (default:OptimumParams)
% Title (optional): The title of the plot
%
% outputs:
% OptimizedParameters
%
% Open Source ECG Toolbox, version 2.0, April 2008
% Released under the GNU General Public License
% Copyright (C) 2008  Reza Sameni
% Sharif University of Technology, Tehran, Iran -- GIPSA-LAB, INPG, Grenoble, France
% reza.sameni@gmail.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.

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

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

% Last Modified by GUIDE v2.5 08-Oct-2007 13:42:43

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

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

% --- Executes just before ECGBeatFitter is made visible.
function ECGBeatFitter_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to ECGBeatFitter (see VARARGIN)

%setappdata(hObject,'SelectedIndeces',0);
mn = varargin{1};
sd = varargin{2};
phase = varargin{3};
handles.mdlError = 'ModelErrorPercentage';
if (nargin>6),
    handles.ExpParamName = varargin{4};
else
    handles.ExpParamName = 'OptimumParams';
end

if (nargin>7),
    set(handles.text3,'String',varargin{5});
end

handles.Linehandles = [];
handles.Initmodhandle = [];
handles.Optmodhandle = [];

handles.SelectedIndeces = [];
handles.ECGmean = mn;
handles.ECGsd = sd;
handles.ECGphase = phase;
handles.OptimizedParameters = [];

axes(handles.axes1);

errorbar(mn,sd/2,'b');
hold on;
plot(mn,'r','linewidth',2);
grid on;
xlabel('Sample index');
ylabel('Amplitude (mV)');
set(handles.axes1,'ButtonDownFcn',@MyButtonDownFcn);
ECGcurve = get(handles.axes1,'Children');
set(ECGcurve,'ButtonDownFcn',@MyButtonDownFcn);
axis tight;
set(handles.figure1,'Pointer','crosshair');

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

% Update handles structure
guidata(hObject, handles);

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

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

% Get default command line output from handles structure

% varargout{1} = handles.output;
varargout{1} = handles.OptimizedParameters;

% --- Executes on button press in Reset.
function Reset_Callback(hObject, eventdata, handles)
% hObject    handle to Reset (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

data = guidata(gcbo);

% delete(data.Initmodhandle);
delete(data.Optmodhandle);
% data.Initmodhandle = [];
data.Optmodhandle = [];

N = length(data.SelectedIndeces);
for i = 1:N
    delete(data.Linehandles(i));
end
data.SelectedIndeces = [];
data.Linehandles = [];

set(data.kernelnumber,'String','');
set(data.opterror,'String','');

guidata(gcbo,data);

% --- Executes on button press in Optimize.
function Optimize_Callback(hObject, eventdata, handles)
% hObject    handle to Optimize (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

data = guidata(gcbo);

% delete(data.Initmodhandle);
delete(data.Optmodhandle);

% localpeaks = zeros(size(data.ECGmean));
I = max(round(data.SelectedIndeces),1);
I = min(I,length(data.ECGmean));

P = length(I);
% localpeaks(I) = 1;

tetai = data.ECGphase(I(1:P));
alphai = 1.2*data.ECGmean(I(1:P));
bi = .04*ones(size(alphai));

options = optimset('TolX',1e-4,'TolFun',1e-4,'MaxIter',100);
InitParams = [alphai bi tetai];

OptParams = nlinfit(data.ECGphase,data.ECGmean,@ECGModel,InitParams,options);
% OptParams = lsqnonlin(@(InitParams) ECGModelError(InitParams,ECGmn,Phasemn,0),InitParams,InitParams-2,InitParams+2,options);
% Model0 = ECGModelError(InitParams,data.ECGmean,data.ECGphase,1);
Model = ECGModelError(OptParams,data.ECGmean,data.ECGphase,1);

ax = axis;
% data.Initmodhandle = plot(Model0,'g');
data.Optmodhandle = plot(Model,'k','LineWidth',2);

L = length(OptParams);

ph = OptParams(2*L/3+1:L);
for i = 1:L/3,
    if(ph(i)<min(data.ECGphase) || ph(i)>max(data.ECGphase))
        ph(i) = mean(data.ECGphase); % place the possible erroneous estimate at the middle of the ECG beat (close to the R-peak)
    end

    pos = find(data.ECGphase>=ph(i),1);

    I = max(pos,1);
    I = min(I,length(data.ECGphase));
    data.SelectedIndeces(i) = I;
    set(data.Linehandles(i),'XData',[I I]);
end
% % %
% % % P = length(I);
% % % % localpeaks(I) = 1;
% % %
% % % tetai = data.ECGphase(I(1:P));

axis(ax);

data.OptimizedParameters = OptParams;

er = 100*mean((Model-data.ECGmean).^2)/mean(data.ECGmean.^2);

set(data.kernelnumber,'String',length(data.SelectedIndeces));
set(data.opterror,'String',round(100*er)/100);

% Modified 25/1/2019 by Reza Sameni:
% assignin('base',handles.mdlError,er);
assignin('caller',handles.mdlError,er);

guidata(gcbo,data);

% --- Executes on button press in ExportData.
function ExportData_Callback(hObject, eventdata, handles)
% hObject    handle to ExportData (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Modified 25/1/2019 by Reza Sameni:
% assignin('base',handles.ExpParamName,handles.OptimizedParameters);
assignin('caller',handles.ExpParamName,handles.OptimizedParameters);

% handles.output = 1; % Function has finished execution
% guidata(hObject, handles);
uiresume(handles.figure1);

function MyButtonDownFcn(hObject, eventdata, handles)
pn = get(gca,'Currentpoint');
% hndl = plot(pn(1,1),pn(1,2),'ro');
ax = axis;
ln = line([pn(1,1),pn(1,1)],[ax(3),ax(4)]);
set(ln,'Color',[0 1 .5]);

data = guidata(gcbo);
data.SelectedIndeces = [data.SelectedIndeces pn(1,1)];
data.Linehandles = [data.Linehandles ln];

guidata(gcbo,data);

% --------------------------------------------------------------------
function Helpmenu_Callback(hObject, eventdata, handles)
% hObject    handle to Helpmenu (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% --------------------------------------------------------------------
function About_1_Callback(hObject, eventdata, handles)
% hObject    handle to About_1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
AboutPath = which('About.htm');
web(AboutPath);

% --------------------------------------------------------------------
function Help_1_Callback(hObject, eventdata, handles)
% hObject    handle to Help_1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

HelpPath = which('Help.htm');
web(HelpPath);

% --- Executes on button press in Exit.
function Exit_Callback(hObject, eventdata, handles)
% hObject    handle to Exit (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% assignin('base',handles.ExpParamName,[]);
uiresume(handles.figure1);


function Write_ActuatorPower(FileWritingSetup)
% Write_Power(FileWritingSetup)
%
% FUNCTION:
%
%   This function writes a function that computes the power being used by
%   the actuators on the robot.
%
%
% 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.
%
%   Power = struct with a field for each actuator.
%
%   Actuators = [P x 2] cell array giving the names of each of the
%       actuators and their comments.
%
%   Directory = the name of the directory to write the files in.
%
%
% OUTPUTS:
%   This function will write the function power()
%
%
% 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 8, 2013
% Cornell University
%
% See also DERIVE_EOM

States = FileWritingSetup.States;
Power = FileWritingSetup.Power;
Directory = FileWritingSetup.Directory;
Actuators = FileWritingSetup.Actuators;

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

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

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

fprintf(fid,'function Power = actuatorPower(States, Actuators, Phase)\n');
fprintf(fid,'%% function Power = actuatorPower(States, Actuators, Phase)\n');
fprintf(fid,'%%\n%% Computer Generated File -- DO NOT EDIT \n');
fprintf(fid,['%%\n%% This function was created by the function '...
    'Write_Power()\n']);
fprintf(fid,['%% ' datestr(now) '\n%%\n']);
fprintf(fid,'%%  ARGUMENTS: \n');
fprintf(fid,'%%   States = [Nstate x Ntime] matrix of states \n');
fprintf(fid,'%%   Actuator = [Nactuator x Ntime] matrix of actuator values \n');
fprintf(fid,'%%   Phase = {''D'',''F'',''S1'',S2''} or omit. \n');
fprintf(fid,'%%       Since the ankle torques are acting against the ground, they should \n');
fprintf(fid,'%%       be set to zero when a given leg is in flight. If Phase is included, \n');
fprintf(fid,'%%       then this function will force these inputs to be zero. If it is \n');
fprintf(fid,'%%       ommitted, then it is assumed that the user has done that error \n');
fprintf(fid,'%%       checking. \n');
fprintf(fid,'%% \n');
fprintf(fid,'%%  RETURNS: \n');
fprintf(fid,'%%   Power = a struct with fields for each actuator: \n');
fprintf(fid,'%%       leg one (linear force actuator) \n');
fprintf(fid,'%%       leg two (linear force actuator) \n');
fprintf(fid,'%%       ankle one (torque actuator) \n');
fprintf(fid,'%%       ankle two (torque actuator) \n');
fprintf(fid,'%%       hip (torque from leg one on leg two \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');

fprintf(fid,'if nargin==2\n');
fprintf(fid,'    Phase = '''';\n');
fprintf(fid,'end\n');
fprintf(fid,' \n');

for idxAct=1:size(Actuators,1)

    if strcmp(Actuators{idxAct,1},'T1')
        fprintf(fid,'if strcmp(Phase,''F'') || strcmp(Phase, ''S2'') \n');
        fprintf(fid,['    ' Actuators{idxAct,1} ' = zeros(1,size(Actuators,2)); %%Foot One not in contact with ground!\n']);
        fprintf(fid,'else\n');
        fprintf(fid,['    ' Actuators{idxAct,1} ' = Actuators(' num2str(idxAct) ',:); %% ' Actuators{idxAct,2} '\n']);
        fprintf(fid,'end\n');
    elseif strcmp(Actuators{idxAct,1},'T2')
        fprintf(fid,'if strcmp(Phase,''F'') || strcmp(Phase, ''S1'') \n');
        fprintf(fid,['    ' Actuators{idxAct,1} ' = zeros(1,size(Actuators,2)); %%Foot Two not in contact with ground!\n']);
        fprintf(fid,'else\n');
        fprintf(fid,['    ' Actuators{idxAct,1} ' = Actuators(' num2str(idxAct) ',:); %% ' Actuators{idxAct,2} '\n']);
        fprintf(fid,'end\n');
    else
        fprintf(fid,[Actuators{idxAct,1} ' = Actuators(' num2str(idxAct) ',:); %% ' Actuators{idxAct,2} '\n']);
    end

end %idxAct
fprintf(fid,'\n');

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

FieldNames = fieldnames(Power);
for i=1:length(FieldNames)
    fprintf(fid,['Power.' FieldNames{i} ' = ' vectorize(Power.(FieldNames{i})) ';\n']);
end
fprintf(fid,'\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


% drawobject ... draws a (calibration) object of a specific type
%
% [fig] = drawobject(X, ctype, fig, {color})
%
% X ...... 3xn matrix containg the object's corner points
%          if X is 4xn only the first 3 rows are used
% ctype .. a string specifying the type of the object
%          according to the type the appropriate drawing functions is called
%          supported types are:
%          'cube'    - a cube
%          'octagon' - a planar octagon
%          'cloud'   - a point cloud
% fig .... figure handle
% color .. color of plotting; defaults to blue
%
% fig .... returns the figure handle
%
% $Id: drawobject.m,v 2.0 2003/06/19 12:07:12 svoboda Exp $

function [fig] = drawobject(X,ctype,fig,color)

if nargin < 4
  color = 'b';	% default color
end

switch ctype
case 'cube',
    drawcube(X,fig,color);
case 'octagon',
    drawoctagon(X,fig,color);
case 'cloud',
    drawcloud(X,fig,color);
otherwise,
    error('unknown object type: ', ctype)
end

return


function v = nan2zero(v)

ind = isnan(v.x);
v.x(ind) = 0;
v.y(ind) = 0;
v.z(ind) = 0;

end


function v = load_sequence(inpat, from, to)

% load first frame to get dimensions, channels
im = single(imread(sprintf(inpat,from)));
sz = [size(im,1) size(im,2)];
ch =  size(im,3);
nf = to - from + 1;

% allocate video volume
v = single(zeros(sz(1), sz(2), ch, nf));

% load sequence
disp(sprintf('Reading %s', inpat))
for i = [from:to],

	filename = sprintf(inpat,i);
%	disp(sprintf('Reading %s', filename))
	im = single(imread(filename));
	v(:,:,:,i - from + 1) = im;

end