set expand=True to fix casadi NaN issues, remove debugging stuff