Nonlinear Optimal Control of Automated Vehicles in a Connected Environment