https://9lib.org/document/ozl63rlz-hierarchical-optimization-control-approach-cooperative-connected-automated-vehicles.html