Optimization based 2D platoon control with multiple behaviors implemented through a finite state machine