\begin{Verbatim}[commandchars=\\\{\}]
\PYG{n}{slow\PYGZus{}threshold} \PYG{o}{=} \PYG{l+m+mi}{5}
\PYG{n}{safe\PYGZus{}stop\PYGZus{}headway} \PYG{o}{=} \PYG{l+m+mi}{15}
\PYG{n}{forward\PYGZus{}horizon} \PYG{o}{=} \PYG{l+m+mi}{100}
\PYG{n}{cruise\PYGZus{}speed} \PYG{o}{=} \PYG{l+m+mf}{13.9}
\PYG{n}{last\PYGZus{}target\PYGZus{}speed} \PYG{o}{=} \PYG{n}{cruise\PYGZus{}speed}
\PYG{n}{lane\PYGZus{}width} \PYG{o}{=} \PYG{l+m+mf}{4.5}
\PYG{n}{time\PYGZus{}threshold} \PYG{o}{=} \PYG{l+m+mf}{0.5}
\PYG{n}{timestep} \PYG{o}{=} \PYG{l+m+mf}{0.05}

\PYG{n}{in\PYGZus{}lane} \PYG{o}{=} \PYG{n+nb}{filter}\PYG{p}{(}\PYG{k}{lambda} \PYG{n}{x}\PYG{p}{:} \PYG{n+nb}{abs}\PYG{p}{(}\PYG{n}{x}\PYG{o}{.}\PYG{n}{position}\PYG{o}{.}\PYG{n}{y}\PYG{p}{)} \PYG{o}{\PYGZlt{}} \PYG{n}{lane\PYGZus{}width} \PYG{o}{/} \PYG{l+m+mi}{2}\PYG{p}{,} \PYG{n}{objects}\PYG{p}{)}
\PYG{n}{within\PYGZus{}horizon} \PYG{o}{=} \PYG{n+nb}{filter}\PYG{p}{(}\PYG{k}{lambda} \PYG{n}{x}\PYG{p}{:} \PYG{n}{x}\PYG{o}{.}\PYG{n}{position}\PYG{o}{.}\PYG{n}{x} \PYG{o}{\PYGZlt{}} \PYG{n}{forward\PYGZus{}horizon}\PYG{p}{,} \PYG{n}{in\PYGZus{}lane}\PYG{p}{)}
\PYG{n}{slow} \PYG{o}{=} \PYG{n+nb}{list}\PYG{p}{(}\PYG{n+nb}{filter}\PYG{p}{(}\PYG{k}{lambda} \PYG{n}{x}\PYG{p}{:} \PYG{n}{x}\PYG{o}{.}\PYG{n}{speed} \PYG{o}{\PYGZlt{}} \PYG{n}{slow\PYGZus{}threshold}\PYG{p}{,} \PYG{n}{within\PYGZus{}horizon}\PYG{p}{))}

\PYG{k}{if} \PYG{n+nb}{len}\PYG{p}{(}\PYG{n}{slow}\PYG{p}{)} \PYG{o}{\PYGZgt{}} \PYG{l+m+mi}{0}\PYG{p}{:}
    \PYG{n}{closest\PYGZus{}agent} \PYG{o}{=} \PYG{n+nb}{sorted}\PYG{p}{(}\PYG{n}{slow}\PYG{p}{,} \PYG{n}{key}\PYG{o}{=}\PYG{k}{lambda} \PYG{n}{x}\PYG{p}{:} \PYG{n}{x}\PYG{o}{.}\PYG{n}{position}\PYG{o}{.}\PYG{n}{x}\PYG{p}{)[}\PYG{l+m+mi}{0}\PYG{p}{]}

    \PYG{n}{distance\PYGZus{}to\PYGZus{}closest} \PYG{o}{=} \PYG{n}{distance\PYGZus{}to\PYGZus{}closest\PYGZus{}agent\PYGZus{}vertex}\PYG{p}{(}\PYG{n}{closest\PYGZus{}agent}\PYG{p}{)}
    \PYG{n}{distance\PYGZus{}to\PYGZus{}safe} \PYG{o}{=} \PYG{n}{distance\PYGZus{}to\PYGZus{}closest} \PYG{o}{\PYGZhy{}} \PYG{n}{safe\PYGZus{}stop\PYGZus{}headway}

    \PYG{k}{if} \PYG{n}{distance\PYGZus{}to\PYGZus{}safe} \PYG{o}{\PYGZlt{}} \PYG{l+m+mf}{0.1}\PYG{p}{:}
        \PYG{n}{target\PYGZus{}speed} \PYG{o}{=} \PYG{l+m+mi}{0}
    \PYG{k}{else}\PYG{p}{:}
        \PYG{n}{speed\PYGZus{}of\PYGZus{}closest} \PYG{o}{=} \PYG{n}{closest\PYGZus{}agent}\PYG{o}{.}\PYG{n}{speed}

        \PYG{k}{if} \PYG{n}{speed\PYGZus{}of\PYGZus{}closest} \PYG{o}{\PYGZlt{}} \PYG{l+m+mf}{0.5}\PYG{p}{:}
            \PYG{n}{speed\PYGZus{}of\PYGZus{}closest} \PYG{o}{=} \PYG{l+m+mi}{0}

        \PYG{n}{accel} \PYG{o}{=} \PYG{p}{(}\PYG{n}{speed\PYGZus{}of\PYGZus{}closest} \PYG{o}{**} \PYG{l+m+mi}{2} \PYG{o}{\PYGZhy{}} \PYG{n}{last\PYGZus{}target\PYGZus{}speed} \PYG{o}{**} \PYG{l+m+mi}{2}\PYG{p}{)} \PYG{o}{/} \PYG{p}{(}\PYG{l+m+mi}{2} \PYG{o}{*} \PYG{n}{distance\PYGZus{}to\PYGZus{}safe}\PYG{p}{)}
        \PYG{n}{target\PYGZus{}speed} \PYG{o}{=} \PYG{n}{last\PYGZus{}target\PYGZus{}speed} \PYG{o}{+} \PYG{n}{accel} \PYG{o}{*} \PYG{n}{timestep}

\PYG{k}{else}\PYG{p}{:}
    \PYG{n}{target\PYGZus{}speed} \PYG{o}{=} \PYG{n}{last\PYGZus{}target\PYGZus{}speed} \PYG{o}{+} \PYG{n}{target\PYGZus{}accel} \PYG{o}{*} \PYG{n}{timestep}
\end{Verbatim}