Algorithm 2 ComputeReferenceHorizon: Nominal or Re-Entry (arc-length resampling)
Require: State ${\mathbf{x}}_k={[x_k,y_k,{\theta }_k]}^{\top }$, nominal path $\mathcal{P}={\left\{(x_i,y_i,{\theta }_i)\right\}}_{i=1}^M$, horizon $N_p$, band $\delta =0.05\mathrm{m}$, look-ahead $L_{\mathrm{smooth}}$
1:	Find closest point ${\mathbf{p}}_c=(x_c,y_c,{\theta }_c)\in \mathcal{P}$ to $(x_k,y_k)$; compute $e_y$
2:	if $|e_y| \le \delta$then	▹ Inside band
3:	${\mathcal{H}}_k\leftarrow \{{\mathbf{x}}_{c+1}^{ref},{\mathbf{x}}_{c+2}^{ref},\dots ,{\mathbf{x}}_{c+N_p}^{ref}\}$
4:	if $|{\mathcal{H}}_k| <N_p$ then
5:	Pad with ${\mathbf{x}}_M^{ref}$ until $|{\mathcal{H}}_k| =N_p$
6:	return ${\mathcal{H}}_k$	▹ Outside band: re-entry
7:	Select smoothing point ${\mathbf{p}}_s=(x_s,y_s,{\theta }_s)\in \mathcal{P}$ such that $\parallel {\mathbf{p}}_s-{\mathbf{p}}_c\parallel \ge L_{\mathrm{smooth}}$
8:	Define first point (robot): ${\mathbf{p}}_r=(x_k,y_k,{\theta }_r)$ with ${\theta }_r={tan}^{-1}\left({\displaystyle \frac{y_s-y_k}{x_s-x_k}}\right)$
9:	Build seed $\mathcal{S}=\{{\mathbf{p}}_r,{\mathbf{p}}_s,{\mathbf{x}}_{s+1}^{ref},\dots ,{\mathbf{x}}_{s+N_p-2}^{ref}\}$
10:	Compute cumulative arc-length $s_0=0$, $s_j=s_{j-1}+\parallel [x_j,y_j]-[x_{j-1},y_{j-1}]\parallel$
11:	Fit interpolants $x\left(s\right),y\left(s\right),\theta \left(s\right)$ (cubic if $\left|\mathcal{S}\right|\ge 4$, else linear)
12:	Resample at uniform arc steps: $s_{\ell }=\ell ·\Delta s,\ell =0,\dots ,L/\Delta s$
13:	Define ${\mathcal{H}}_k={\left\{(x\left(s_{\ell }\right),y\left(s_{\ell }\right),\theta \left(s_{\ell }\right))\right\}}_{\ell =1}^{N_p}$
14:	if $|{\mathcal{H}}_k| <N_p$  then
15:	Pad with ${\mathbf{x}}_M^{ref}$ until $|{\mathcal{H}}_k| =N_p$
16:	return ${\mathcal{H}}_k$