Switched Systems Optimal Control Formulation and a Two Stage Optimization Methodology