clear all;

clc;

N = 9; % N should match the number of elements in epsi

% Constants (example values)

D = 22.23;
B = 0.0464;
alpha = 0.69813;
Ko1 = 1486899;
Ki1 = 1355496;
fi = 0.5232;
gama_o1 = 1;
Ri = 63.0400;
epsi = [0, 0.3926990817, 0.7853981634, 1.1780972451, 1.5707963268, 1.9634954085,
2.3561944902, 2.7488935719, 3.1415926536];
gama_i1 = 1;
Mo = 40083.35;
Mi = -893.45;
Fc1 = 3100.8;
fo = 0.5232;
f_a = 22500;
f_r = 0;
m = 0;

% Initialize the result storage

optimal_solutions = [];
error_values = [];

% Fix the random seed for reproducibility

rng(1);

% Function to generate equations

function [fs, xs] = generate_equations(D, B, alpha, Ko1, Ki1, fi, gama_o1, Ri,
epsi, gama_i1, Mo, Mi, Fc1, fo, delta_a, delta_r, theta)
fs = [];
xs = [];
syms X1 X2 delta_i delta_o;

F1 = ((B*D*sin(alpha) + delta_a) + (Ri*theta*cos(epsi) - X1))^2 +

((B*D*cos(alpha) + delta_r) - X2)^2 - ((fi - 0.5) * D + delta_i)^2;
F2 = (X1)^2 + (X2)^2 - ((fi - 0.5) * D + delta_o)^2;
V1 = ((gama_o1 * Mo * X2) / D - Ko1 * (delta_o)^1.5 * X1) / ((fo - 0.5) * D +
delta_o);
U1 = (Ki1 * (delta_i)^1.5 * (B * D * sin(alpha) + delta_a + Ri * theta *
cos(epsi) - X1) - (gama_i1 * Mi / D) * (B * D * cos(alpha) + delta_r - X2)) / ((fi
- 0.5) * D + delta_i);
F3 = V1 + U1;
V2 = (Ko1 * (delta_o)^1.5 * X2 + (gama_o1 * Mo * X1) / D) / ((fo - 0.5) * D +
delta_o);
U2 = (Ki1 * (delta_i)^1.5 * (B * D * cos(alpha) + delta_r - X2) + (gama_i1 * Mi
/ D) * (B * D * sin(alpha) + delta_a + Ri * theta * cos(epsi) - X1)) / ((fi - 0.5)
* D + delta_i);
F4 = V2 - U2 - Fc1;

fs = [fs; F1; F2; F3; F4];

xs = [xs; X1; X2; delta_i; delta_o];
end
% Run optimization for the first epsi to get the fixed delta_r, delta_a, and theta
try
% Generate distributed random numbers
delta_r = unifrnd(0, 0.1); % Uniform distribution between 0 and 0.1
delta_a = unifrnd(0, 0.033);
theta = unifrnd(0, 0.1);

% Store the optimal solution

delta_r_fixed = delta_r;
delta_a_fixed = delta_a;
theta_fixed = theta;

% Display the optimal result for the epsi

disp('Optimal result for the epsi:');
fprintf('delta_r: %.5f, delta_a: %.5f, theta: %.5f\n', delta_r_fixed,
delta_a_fixed, theta_fixed);

catch ME
% Display any errors that occurred
disp('Error occurred:');
disp(ME.message);
end

% Loop through each epsi with fixed delta_r, delta_a, and theta
for k = 1:length(epsi)
try
% Generate equations and variables
[fs, xs] = generate_equations(D, B, alpha, Ko1, Ki1, fi, gama_o1, Ri,
epsi(k), gama_i1, Mo, Mi, Fc1, fo, delta_a_fixed, delta_r_fixed, theta_fixed);

% Create function handle for fs

f_func = matlabFunction(fs, 'Vars', [{xs}, {sym('delta_a')},
{sym('delta_r')}, {sym('theta')}]);

% Create function handle for jacobian(fs, xs)

jac_func = matlabFunction(jacobian(fs, xs), 'Vars', [{xs},
{sym('delta_a')}, {sym('delta_r')}, {sym('theta')}]);

% Initial guess
x = repmat(0.1, [numel(xs), 1]); % Initial guess for all variables

% Iterative solution process

for ii = 1:100 % Limit the number of iterations for convergence
f = f_func(x, delta_a_fixed, delta_r_fixed, theta_fixed);
jj = jac_func(x, delta_a_fixed, delta_r_fixed, theta_fixed);
regularization_factor = 1e-7; % Fine-tuned regularization factor
jj = jj + regularization_factor * eye(size(jj));

% Solve the linear system

delta_x = jj \ f;
x = x - delta_x;
err = max(abs(delta_x));

% Enforce the values to be within the range [0, 0.999]

x = min(max(x, 0), 0.999);

% Check convergence criteria

if err < 1e-5
break;
 end
end

% Store the optimal solution and associated error

optimal_solutions = [optimal_solutions; epsi(k), delta_r_fixed,
delta_a_fixed, theta_fixed, x'];
error_values = [error_values; epsi(k), err];

% Print f1, f2, f3, f4 values

fprintf('For epsi = %.5f\n', epsi(k));
fprintf('f1 = %.5f\n', double(f(1)));
fprintf('f2 = %.5f\n', double(f(2)));
fprintf('f3 = %.5f\n', double(f(3)));
fprintf('f4 = %.5f\n', double(f(4)));

% Calculate and print Fa, Fr, and M

X1_val = x(1);
X2_val = x(2);
delta_i_val = x(3);
delta_o_val = x(4);

% Define anonymous functions for Fa, Fr, and M

Fa = @(delta_a, delta_r, theta) ...
(((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(1)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)-((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)) + ...
2* (((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(2)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)-((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)) + ...
2* (((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(3)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)-((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)) + ...
2* (((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(4)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)-((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)) + ...
2* (((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(5)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)-((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)) + ...
2* (((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(6)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)-((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)) + ...
2* (((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(7)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)-((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)) + ...
2* (((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(8)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)-((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)) + ...
(((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(9)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)-((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)) ;
Fr = @(delta_a, delta_r, theta) ...
(((Ki1*(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)) * optimal_solutions(1,
6)^1.5)+((gama_o1*Mo)/D)* ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(1)) -
optimal_solutions(1, 4)))))/ ((fo - 0.5)*D + optimal_solutions(1, 6)) + ...
2*(((Ki1*(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)) *
optimal_solutions(1, 6)^1.5)+((gama_o1*Mo)/D)* ((B*D*sin(alpha) + delta_a +
Ri*theta*cos(epsi(2)) - optimal_solutions(1, 4)))))/ ((fo - 0.5)*D +
optimal_solutions(1, 6)) + ...
2*(((Ki1*(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)) *
optimal_solutions(1, 6)^1.5)+((gama_o1*Mo)/D)* ((B*D*sin(alpha) + delta_a +
Ri*theta*cos(epsi(3)) - optimal_solutions(1, 4))))) / ((fo - 0.5)*D +
optimal_solutions(1, 6)) + ...
2*(((Ki1*(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)) *
optimal_solutions(1, 6)^1.5)+((gama_o1*Mo)/D)* ((B*D*sin(alpha) + delta_a +
Ri*theta*cos(epsi(4)) - optimal_solutions(1, 4)))))/ ((fo - 0.5)*D +
optimal_solutions(1, 6)) + ...
2*(((Ki1*(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)) *
optimal_solutions(1, 6)^1.5)+((gama_o1*Mo)/D)* ((B*D*sin(alpha) + delta_a +
Ri*theta*cos(epsi(5)) - optimal_solutions(1, 4))))) / ((fo - 0.5)*D +
optimal_solutions(1, 6)) + ...
2*(((Ki1*(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)) *
optimal_solutions(1, 6)^1.5)+((gama_o1*Mo)/D)* ((B*D*sin(alpha) + delta_a +
Ri*theta*cos(epsi(6)) - optimal_solutions(1, 4)))))/ ((fo - 0.5)*D +
optimal_solutions(1, 6)) + ...
2*(((Ki1*(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)) *
optimal_solutions(1, 6)^1.5)+((gama_o1*Mo)/D)* ((B*D*sin(alpha) + delta_a +
Ri*theta*cos(epsi(7)) - optimal_solutions(1, 4)))))/ ((fo - 0.5)*D +
optimal_solutions(1, 6)) + ...
2*(((Ki1*(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)) *
optimal_solutions(1, 6)^1.5)+((gama_o1*Mo)/D)* ((B*D*sin(alpha) + delta_a +
Ri*theta*cos(epsi(8)) - optimal_solutions(1, 4)))))/ ((fo - 0.5)*D +
optimal_solutions(1, 6)) + ...
(((Ki1*(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5)) * optimal_solutions(1,
6)^1.5)+((gama_o1*Mo)/D)* ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(9)) -
optimal_solutions(1, 4))))) / ((fo - 0.5)*D + optimal_solutions(1, 6)) ;

M = @(delta_a, delta_r, theta) ...

(((((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(1)) -optimal_solutions(1,
4)) * optimal_solutions(1, 6)^1.5)-((Ri*gama_i1*Mi)/D) * (B*D*cos(alpha) + delta_r
- optimal_solutions(1, 5))))/ ((fi - 0.5)*D + optimal_solutions(1, 6)))+
gama_i1*Mi*fi))*cos(epsi(1)) +...
2 * (((((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(2)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5) - ((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)))+ gama_i1*Mi*fi))*cos(epsi(2)) +...
2 * (((((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(3)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)- ((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)))+ gama_i1*Mi*fi))*cos(epsi(3)) +...
2 * (((((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(4)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)- ((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)))+ gama_i1*Mi*fi))*cos(epsi(4)) +...
2 * (((((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(5)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)- ((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)))+ gama_i1*Mi*fi))*cos(epsi(5)) +...
2 * (((((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(6)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)- ((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)))+ gama_i1*Mi*fi))*cos(epsi(6)) +...
2 * (((((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(7)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)- ((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)))+ gama_i1*Mi*fi))*cos(epsi(7)) +...
2 * (((((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(8)) -
optimal_solutions(1, 4)) * optimal_solutions(1, 6)^1.5)- ((Ri*gama_i1*Mi)/D) *
(B*D*cos(alpha) + delta_r - optimal_solutions(1, 5))))/ ((fi - 0.5)*D +
optimal_solutions(1, 6)))+ gama_i1*Mi*fi))*cos(epsi(8)) +...
(((((Ki1 * ((B*D*sin(alpha) + delta_a + Ri*theta*cos(epsi(9)) -optimal_solutions(1,
4)) * optimal_solutions(1, 6)^1.5) -((Ri*gama_i1*Mi)/D) * (B*D*cos(alpha) + delta_r
- optimal_solutions(1, 5))))/ ((fi - 0.5)*D + optimal_solutions(1, 6)))+
gama_i1*Mi*fi))*cos(epsi(9)) ;
% Evaluate Fa, Fr, and M at the optimal solution
Fa_val = Fa(delta_a_fixed, delta_r_fixed, theta_fixed);
Fr_val = Fr(delta_a_fixed, delta_r_fixed, theta_fixed);
M_val = M(delta_a_fixed, delta_r_fixed, theta_fixed);

% Compare with f_a, f_r, and m

if abs(Fa_val - f_a) < 1e-5 && abs(Fr_val - f_r) < 1e-5 && abs(M_val - m) <
1e-5
disp('Optimal solution satisfies all conditions:');
fprintf('delta_r: %.5f, delta_a: %.5f, theta: %.5f\n', delta_r_fixed,
delta_a_fixed, theta_fixed);
fprintf('X1: %.5f, X2: %.5f, delta_i: %.5f, delta_o: %.5f\n', X1_val,
X2_val, delta_i_val, delta_o_val);
fprintf('Fa: %.5f, Fr: %.5f, M: %.5f\n', Fa_val, Fr_val, M_val);
else
disp('Optimal solution does not satisfy all conditions:');
fprintf('delta_r: %.5f, delta_a: %.5f, theta: %.5f\n', delta_r_fixed,
delta_a_fixed, theta_fixed);
fprintf('X1: %.5f, X2: %.5f, delta_i: %.5f, delta_o: %.5f\n', X1_val,
X2_val, delta_i_val, delta_o_val);
fprintf('Fa: %.5f (expected %.5f), Fr: %.5f (expected %.5f), M: %.5f
(expected %.5f)\n', Fa_val, f_a, Fr_val, f_r, M_val, m);
end

catch ME
% Display any errors that occurred
disp(['Error occurred for epsi = ', num2str(epsi(k)), ':']);
disp(ME.message);
end
end

% Display all optimal solutions and associated errors

disp('All optimal solutions and associated errors:');
disp(' epsi, delta_r, delta_a, theta, X1, X2, delta_i,
delta_o');
disp(optimal_solutions);
disp('epsi, error');
disp(error_values);