controller_swarm1(densities, flows, celldata, ts, idx)

function new_flows = controller_swarm1(densities, flows, celldata, ts, idx)
% CONTROLLER_SWARM1 - implementation of SWARM 1 ramp metering strategy.
%
% Call:   new_flows = controller_swarm1(densities, flows, celldata, ts, idx)
%
% Parameters:
%             densities - vector of densities;
%             flows     - vector of on-ramp flows, same size as 'densities';
%             celldata  - array of freeway cell structures, whose length
%                         must be the same as size of 'densities';
%             ts        - sampling period;
%             idx       - index of the cell with our on-ramp.
%
% Returns:   new_flows - updated vector of on-ramp flows.
%
% REMARK: currently, we only do 1 time step forecast.
%
% Last modified:   11/17/2006.

%
% Alex Kurzhanskiy   <[e-mail]>
%

global CTMSIM_ORC;

L = size(densities, 1);
M = size(flows, 1);
N = size(celldata, 2);

if L ~= N
  error('CONTROLLER_SWARM1: number of densities does not match number of cells.');
end

if M ~= N
  error('CONTROLLER_SWARM1: number of flows does not match number of cells.');
end

if (idx < 1) | (idx > N)
  error('CONTROLLER_SWARM1: invalid cell index.');
end

new_flows = flows;  % initialize flow vector with current values

if ~isstruct(CTMSIM_ORC)  % SWARM1 invoked for the first time
  CTMSIM_ORC.nodelete          = 1;
  CTMSIM_ORC.swarm1_denhistory = densities;
  CTMSIM_ORC.swarm1_currcell   = idx;
  CTMSIM_ORC.swarm1_Tp         = round(celldata(idx).ORmlcontroller.Tp);
  if CTMSIM_ORC.swarm1_Tp < 2
    CTMSIM_ORC.swarm1_Tp = 2;
  end
  return;  % enough for the first time
end

if idx >= CTMSIM_ORC.swarm1_currcell  % we are at most downstream on-ramp
  CTMSIM_ORC.swarm1_denhistory = [CTMSIM_ORC.swarm1_denhistory densities];
  CTMSIM_ORC.swarm1_Tp         = round(celldata(idx).ORmlcontroller.Tp);
  if CTMSIM_ORC.swarm1_Tp < 2
    CTMSIM_ORC.swarm1_Tp = 2;
  end
end

K  = size(CTMSIM_ORC.swarm1_denhistory, 2);
Tp = CTMSIM_ORC.swarm1_Tp;

if K < Tp  % not enough historic density data
  CTMSIM_ORC.swarm1_currcell = idx;
  return;  % nothing else to do
end

if idx >= CTMSIM_ORC.swarm1_currcell  % we are at most downstream on-ramp
  if K > Tp  % more than enough historic density data
    CTMSIM_ORC.swarm1_denhistory = CTMSIM_ORC.swarm1_denhistory(:, (K-Tp+1):K);
  end

  % compute forecast
  rho  = CTMSIM_ORC.swarm1_denhistory;
  tt   = 1:Tp;
  S    = (sum(rho'))';
  SS   = S;
  for i = 2:Tp
    SS = SS  +  (i * S);
  end
  nom  = Tp * sum(diag(tt) * rho')' - SS;
  dnom = (Tp * sum(tt.^2))  -  ((sum(tt))^2);
  CTMSIM_ORC.swarm1_fdensities = densities + (nom / dnom);
  CTMSIM_ORC.swarm1_zone       = celldata(idx).ORmlcontroller.zone;
  CTMSIM_ORC.swarm1_zonelen    = 0;

  % reinitialize Rex
  Rex = 0;
else
  Rex = CTMSIM_ORC.swarm1_Rex;
  if CTMSIM_ORC.swarm1_zone ~= celldata(idx).ORmlcontroller.zone
    % we enter new zone
    Rex                       = max([CTMSIM_ORC.swarm1_psy (0/CTMSIM_ORC.swarm1_zonelen)]) * Rex;  % FIXME: what is 1/L??
    CTMSIM_ORC.swarm1_zonelen = 0;
    CTMSIM_ORC.swarm1_zone    = celldata(idx).ORmlcontroller.zone;
  end
end

i  = idx;
nv = 0;
L  = 0;
while (i > 0) & ((i == idx) | isempty(celldata(i).ORname))
  l  = abs(celldata(i).PMend - celldata(i).PMstart);  % length of i-th cell
  nc = celldata(i).FDrhocrit * l;  % critical number of vehicles in i-th cell
  n  = CTMSIM_ORC.swarm1_fdensities(i, 1) * l;  % forecasted number of vehicles in i-th cell
  nv = nv  +  (n - nc);
  L  = L + l;
  i  = i - 1;
end

fdes              = flows(idx, 1) - (nv/ts) - (Rex/ts);  % desired flow
f                 = max([0 fdes celldata(idx).ORmlcontroller.Cmin]);
f                 = min([f celldata(idx).ORmlcontroller.Cmax]);

new_flows(idx, 1) = f;  % update flow vector

CTMSIM_ORC.swarm1_Rex      = celldata(idx).ORmlcontroller.phi * f - fdes;
CTMSIM_ORC.swarm1_psy      = celldata(idx).ORmlcontroller.psy;
CTMSIM_ORC.swarm1_zonelen  = CTMSIM_ORC.swarm1_zonelen + L;
CTMSIM_ORC.swarm1_currcell = idx;


return;