A Finite State Machine Based Automated Driving Controller and its Stochastic Optimization