A graph-based algorithm for optimal control of switched systems: An application to car parking