Self-Organizing and Optimal Control for Nonlinear Systems