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

function new_flows = controller_lqi(densities, flows, celldata, ts, idx)
% CONTROLLER_LQI - implementation of LQI controller.
%
% Call:   new_flows = controller_lqi(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.
%
% Last modified:   10/31/2006.

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

global g_ctmGUI;

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

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

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

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

new_flows = flows;  % initialize flow vector with current values

p = round(celldata(idx).ORmlcontroller.TS / ts);  % period
l = abs(celldata(idx).PMend - celldata(idx).PMstart);  % cell length
if densities(idx, 1) > celldata(idx).FDrhocrit
  a = 1   -   ts*((celldata(idx).FDfmax / (celldata(idx).FDrhojam - celldata(idx).FDrhocrit))  /  l);
  r = celldata(idx).ORmlcontroller.R(1, 2);
else
  a = 1   -   ts*((celldata(idx).FDfmax / celldata(idx).FDrhocrit)  /  l);
  r = celldata(idx).ORmlcontroller.R(1, 1);
end
A  = [1 a; 0 a];
B  = [ts/l; ts/l];
Q  = [celldata(idx).ORmlcontroller.Q 0; 0 0];

T1 = A^(p-1);
T2 = polyvalm(ones(p-1, 1), A);

Ap = T1 * A;
Bp = T1 * B;
Qp = (T1 + T2)' * Q * (T1 + T2);
Rp = r  +  B' * T2' * Q * T2 * B;
Sp = A' * T2' * Q * T2 * B;

[P, L, G] = mydare(Ap, Bp, Qp, Rp, Sp);

M  = size(g_ctmGUI.densityData, 2);
if M < 2
  d1 = densities(idx, 1);
else
  d1 = g_ctmGUI.densityData(idx, M-1);
end
d2 = densities(idx, 1);
dc = celldata(idx).FDrhocrit;
 
f  = flows(idx, 1)  -  G * [(d2-dc); (d2-d1)]; % on-ramp flow that we can allow
f  = min([f celldata(idx).ORmlcontroller.Cmax]);
f  = max([f celldata(idx).ORmlcontroller.Cmin 0]);

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

return;