%% Obtain Solution Using Feasibility Mode
% This example shows how to use the feasibility mode of the fmincon 'interior-point' algorithm to obtain a feasible point. To take advantage of automatic differentiation, the example uses the problem-based approach. The example is taken from Problem 9 of Moré [1].
%% Problem Setup
% The problem has a 5-D optimization variable x along with five quadratic constraints. The first x component has a lower bound of 0, and the remaining four components have upper bounds of 0.

x = optimvar("x",5,"LowerBound",[0;-Inf;-Inf;-Inf;-Inf],"UpperBound",[Inf;0;0;0;0]);

% The problem, which comes from the aircraft industry, uses aeronautical terms for the components of x and has specified values of some parameters.

elevator = 0.1; % If elevator were 0, then [0 0 0 0 0] would be a solution
aileron = 0.0;
rudderdf = 0.0;
rollrate = x(1);
pitchrat = x(2);
yawrate = x(3);
attckang = x(4);
sslipang = x(5);

% Create an optimization problem and the constraints.

prob = optimproblem;
prob.Constraints.eq1 = (-3.933*rollrate + 0.107*pitchrat + ...
    0.126*yawrate - 9.99*sslipang - 45.83*aileron - 7.64*rudderdf - ...
    0.727*pitchrat*yawrate + 8.39*yawrate*attckang - ...
    684.4*attckang*sslipang + 63.5*pitchrat*attckang) == 0;
prob.Constraints.eq2 = (-0.987*pitchrat - 22.95*attckang - ...
    28.37*elevator + 0.949*rollrate*yawrate + 0.173*rollrate*sslipang) == 0;
prob.Constraints.eq3 = (0.002*rollrate - 0.235*yawrate + ...
    5.67*sslipang - 0.921*aileron - 6.51*rudderdf - ...
    0.716*rollrate*pitchrat - 1.578*rollrate*attckang + ...
    1.132*pitchrat*attckang) == 0;
prob.Constraints.eq4 = (pitchrat - attckang - ...
    1.168*elevator - rollrate*sslipang) == 0;
prob.Constraints.eq5 = (-yawrate - 0.196*sslipang - ...
    0.0071*aileron + rollrate*attckang) == 0;

% This problem has no objective function, so do not specify prob.Objective.
%% Attempt Solution Without Feasibility Mode
% Attempt to solve the problem using the default solver and parameters, starting from the point [0 0 0 0 0]'.

x0.x = zeros(5,1);
[sol,~,exitflag,output] = solve(prob,x0)

%
% The solver stops prematurely. Increase the iteration limit and function evaluation limit, and then try again.

options = optimoptions("fmincon","MaxIterations",1e4,"MaxFunctionEvaluations",1e4);
[sol,~,exitflag,output] = solve(prob,x0,"Options",options)

%
% The solver converges to an infeasible point.
%% Solve Using Feasibility Mode
% Try again to solve the problem, this time specifying the EnableFeasibilityMode and SubproblemAlgorithm options. Generally, if you need to use feasibility mode, the best approach is to set the SubproblemAlgorithm option to 'cg'.

options = optimoptions(options,"EnableFeasibilityMode",true,...
    "SubproblemAlgorithm","cg");
[sol,~,exitflag,output] = solve(prob,x0,"Options",options)

%
% This time, the solver reports that it reaches a feasible solution. However, the constraint violation in output.constrviolation is not very small. Tighten the constraint tolerance and solve again. To speed the solution process, start from the returned feasible solution.

options.ConstraintTolerance = 1e-8;
[sol,~,exitflag,output] = solve(prob,sol,"Options",options)

% The constraint violation is now quite small. The solver takes only two iterations to reach this improved solution.
%% References
% [1] Moré, J. J. A collection of nonlinear model problems. Proceedings of the AMS-SIAM Summer Seminar on the Computational Solution of Nonlinear Systems of Equations, Colorado, 1988. Argonne National Laboratory MCS-P60-0289, 1989.
% Copyright 2021 The MathWorks, Inc.