A method includes receiving, from connected vehicles, first locations of a first set of vehicles in an initial vehicle configuration; receiving, from a roadside unit, second locations of a second set of vehicles in the initial vehicle configuration; determining locations of connected vehicles and non-connected vehicles in the initial vehicle configuration based on the first locations and the second locations; determining an optimal vehicle configuration comprising desired locations of the connected vehicles and the non-connected vehicles based on the locations of the connected vehicles and the non-connected vehicles in the initial vehicle configuration, and predetermined optimization criteria; determining driving maneuvers to be performed by the connected vehicles to achieve the optimal vehicle configuration; and transmitting the determined driving maneuvers to the connected vehicles.