function [TC,XFlg,out] = rteTC(rte,varargin)
%RTETC Calculate total cost of route with time windows.
%            TC = rteTC(rte,C)
% [TC,XFlg,out] = rteTC(rte,C,cap,twin,rtefeas)
%               = rteTC(rte),  use arguments C,twin,... from previous call
%                              and do not check input for errors
%    rte = vector of single-route vertices
%        = m-element cell array of m routes
%          (to get sum(TC) as output, use vector rte = [rte{:}] as input)
%      C = n x n matrix of costs between n vertices
%     TC = m-element vector of route costs, where 
%          TC(i) = Inf if route i is infeasible
%
% Optional input and output arguments used to determine route feasibility:
%    cap = {q,Q} = cell array of capacity arguments, where
%              q = n-element vector of vertex demands, with depot q(1) = 0
%              Q = maximum route load
%   twin = {ld,TW,st} = cell array of time window arguments, where
%             ld = n or (n+1)-element vector of loading/unloading
%                  timespans, where
%                     ld(rte(1))   = load at depot
%                     ld(n+1)      = unload at depot, if rte(1) == rte(end)
%                = scalar of constant values "ld" for rte(2) ... rte(end)  
%                  and 0 for rte(1); or rte(2) ... rte(end-1) and 0 for
%                   rte(end), if rte(1) == rte(end)
%                = 0, default
%             TW = n or (n+1) x 2 matrix of time windows, where
%                     TW(i,1)      = start of time window for vertex i
%                     TW(i,2)      = end of time window
%                     TW(rte(1),:) = start time window at depot
%                     TW(n+1,:)    = finish time window at depot, 
%                                    if rte(1) = rte(end)
%                = (n+1)-element cell array, if multiple windows, where
%                     TW{i}        = (2 x p)-element vector of p window 
%                                    (start,end) pairs
%             st = (optional) m-element vector of starting times at depot
%                = TW(1,1) or min(TW{1}), default (earliest starting time)
%rtefeas = {'rtefeasfun',P1,P2,...} = cell array specifying user-defined
%          function to test the feasibility of a single route (in addition  
%          to time windows, capacity, and maximum cost), where RTETC 
%          argument out(i) along with user-specified arguments P1,P2,... 
%          are passed to function and a logical value should be returned: 
%                 isfeas = RTEFEASFUN(out(i),P1,P2,...)
%        = {'maxTCfeas',maxTC} is a predefined route feasibility function
%          to test if the total cost of a route (including loading/
%          unloading times "ld") exceeds the maximum waiting time "maxTC"
%          (see below for code)
%XFlg(i) = exitflag
%        =  1, if route is feasible
%        = -1, if infeasible due to capacity
%        = -2, if infeasible due to time windows
%        = -3, if infeasible due to user-defined feasibility function
%    out = m-element struct array of outputs
% out(i) = output structure with fields:
%        .Rte     = route indices, rte{i}
%        .Cost    = cost from vertex j-1 to j, 
%                   Cost(j) = C(r{i}(j-1),r{i}(j)) and Cost(1) = 0
%                 = drive timespan from vertex j-1 to j
%        .Demand  = demands of vertices on route, q(rte{i})
%        .Arrive  = time of arrival
%        .Wait    = wait timespan if arrival prior to beginning of window
%        .Start   = time loading/unloading started (starting time for
%                   route is "Start(1)")
%        .LD      = loading/unloading timespan, ld(rte{i})
%        .Depart  = time of departure (finishing time is "Depart(end)")
%        .Total   = total timespan from departing vtx j-1 to depart. vtx j
%                   (= drive + wait + loading/unloading timespan)
%        .EarlySF = earliest starting and finishing times (default starting
%                   time is "st" and default finish. time is "EarlySF(2)")
%        .LateSF  = latest starting and finishing times
%
% For each route rte{i}, feasibility is determined in the following order:
%    1. Capacity feasibility: SUM(q(rte{i})) <= Q if feasible
%    2. Time window feasiblity: [TCi,ignore,outi] = RTETC(rte{i},C,twin);
%                               TCi < Inf if feasible
%    3. User defined feasibility: isfeas = RTEFEASFUN(outi,P1,P2,...);
%                                 isfeas == true if feasible
%
% Code for example route feasibility function:
%
% function isfeas = maxTCfeas(outi,maxTC)
% %MAXTCFEAS Maximum total cost route feasibility function.
% % isfeas = maxwaitfeas(outi,maxTC)
% %   outi = struct array of outputs from RTETC for single route i
% %          (automatically passed to function)
% %  maxTC = scalar max. total cost (including un/loading times) of route
% %
% % Route is feasible if sum(outi.Total) <= maxTC
% %
% % This function can be used as a template for developing other
% % route feasibility functions.
% 
% % Input error check
% if ~isnumeric(maxTC) || length(maxTC(:)) ~= 1 || maxTC < 0
%    error('"maxTC" must be a nonnegative scalar.')
% end
% 
% % Feasibility test
% if sum(outi.Total) <= maxTC
%    isfeas = true;
% else
%    isfeas = false;
% end
%
%
% Examples:
% % 4-vertex graph
% IJD = [1 -2 1; 2 -3 2; 3 -4 1; 4 -1 1];
% C = dijk(list2adj(IJD))                  %  C =  0   1   2   1
%                                          %       1   0   2   2
%                                          %       2   2   0   1
%                                          %       1   2   1   0
% rte = [1 2 3 4 1];
% TC = rteTC(rte,C)                        % TC =  5
%
% % Different route, same C from previous call
% TC = rteTC([1 2 3 4])                    % TC =  4
%
% % Time-windows
% ld = 0
% TW = [ 6 18       % Start time window at depot
%        8 11
%       12 14
%       15 18
%       18 24]      % Finish time window at depot
% [TC,XFlg,out] = rteTC([1 2 3 4 1],C,[],{ld,TW})
%                                          %   TC =  8
%                                          % XFlg =  1
%                                          %  out = 
%                                          %          Rte: [1 2 3 4 1]
%                                          %         Cost: [0 1 2 1 1]
%                                          %       Demand: []
%                                          %       Arrive: [0 11 13 14 16]
%                                          %         Wait: [0 0 0 1 2]
%                                          %        Start: [10 11 13 15 18]
%                                          %           LD: [0 0 0 0 0]
%                                          %       Depart: [10 11 13 15 18]
%                                          %        Total: [0 1 2 2 3]
%                                          %      EarlySF: [10 18]
%                                          %       LateSF: [10 18]
%
% % Not feasible if total cost exceeds 4
% [TC,XFlg] = rteTC([1 2 3 4 1],C,[],[],{'maxTCfeas',4})
%                                          %   TC = Inf
%                                          % XFlg =  -3
% [TC,XFlg] = rteTC([1 2 3 4 1],C,[],[],{'maxTCfeas',4})
%                                          %   TC = Inf
%                                          % XFlg =  -3

% Copyright (c) 1994-2006 by Michael G. Kay
% Matlog Version 9 13-Jan-2006 (http://www.ie.ncsu.edu/kay/matlog)

% Input Error Checking ****************************************************

persistent C q Q ld TW st rtefeas     % Set to empty

if nargin < 2
   if isempty(C)
      error('Additional input arguments required for first call.')
   else
      isfirstcall = 0;
   end
else
   isfirstcall = 1;
   if length(varargin) < 4
      [varargin{length(varargin)+1:4}] = deal([]);
   end
   [C,cap,twin,rtefeas] = deal(varargin{:});
   [q,Q,ld,TW,st] = deal([]);
end

m = 1;
if iscell(rte), m = length(rte); end
TC = Inf * ones(m,1);                % All routes initialized to infeasible
XFlg = ones(m,1);                    % All flags inialized to feasible

[n,nC] = size(C);

% Route
if isfirstcall & ~isempty(rte) & (~(isreal(rte) | iscell(rte)) | ...
      (~iscell(rte) & (min(size(rte)) ~= 1 | ...
      any(rte(:) < 1 | rte(:) > n))) | ...
      (iscell(rte) & (any(cellfun('prodofsize',rte) ~= ...
      cellfun('length',rte)) | any([rte{:}] < 1 | [rte{:}] > n))))
   error('"rte" not a valid route.')
end

% Cost
if isfirstcall
   if n ~= nC
      error('C must be a square matrix.')
   elseif any(any(C<0))
      error('C must be a non-negative matrix.')
   elseif any(diag(C) ~= 0)
      error('C must have zeros along its diagonal.')
   end
end

% Capacity
if isfirstcall && ~isempty(cap)
   if ~iscell(cap) || length(cap(:)) ~= 2
      error('"cap" must be a two element cell array.')
   end
   q = cap{1}; Q = cap{2};
   if length(q(:)) ~= n
      error(['"q" must be an ',num2str(n),'-element vector.'])
   elseif q(1) ~= 0
      error('Depot''s demand, q(1), should equal 0.');
   elseif length(Q(:)) ~= 1 || Q < 0
      error('Q must be a nonnegative scalar.')
   elseif any(q > Q)
      error('Elements of "q" can not be greater than Q.')
   end
end

% Time window
if isfirstcall && ~isempty(twin)
   if ~iscell(twin) || length(twin(:)) < 1 || length(twin(:)) > 3
      error('"twin" must be a one or three element cell array.')
   end
   ld = twin{1};
   if ~isempty(ld), ld = ld(:)'; end
   if length(twin) > 1, TW = twin{2}; else TW = []; end
   if ~isempty(TW) && ~iscell(TW), TW = padmat2cell(TW); end
   if length(twin) > 2, st = twin{3}; st = st(:); else st = []; end