function Optimize_robot_arm
% Solve the robotic arm problem using fmincon. It visualizes the position
% of the robotic arm at each step of the minimization problem
    % Define the lengths of the robot arm segments
    l1 = 1; % Length of the first arm segment
    l2 = 1; % Length of the second arm segment
    l3 = 1; % Length of the third arm segment

    % Define the desired position of the end effector
    desiredPosition = [1; 2];

    % Objective function to minimize (distance from the desired position)
    objectiveFunction = @(theta) norm([...
        l1*cos(theta(1)) + l2*cos(theta(1) + theta(2)) + l3*cos(theta(1) + theta(2) + theta(3));
        l1*sin(theta(1)) + l2*sin(theta(1) + theta(2)) + l3*sin(theta(1) + theta(2) + theta(3))
        ] - desiredPosition, 2);

    % Initial guess for the joint angles
    initialGuess = [0; 0; 0];

    % Set up optimization options
    options = optimoptions('fmincon','Display', 'iter', 'Algorithm', 'sqp', ...
        'OutputFcn', @outfun);

    % Prepare the figure
    hFig = figure; clf;
    hAx = axes('Parent', hFig);
    title(hAx, 'Robot Arm Optimization Process');
    xlabel(hAx, 'X');
    ylabel(hAx, 'Y');
    hold(hAx, 'on');
    grid(hAx, 'on');
    axis(hAx, 'equal');

    % Set fixed axes limits
    axisLimits = [-3, 3, -3, 3];
    xlim(hAx, axisLimits(1:2));
    ylim(hAx, axisLimits(3:4));

    % Plot the desired position as a green square
    plot(hAx, desiredPosition(1), desiredPosition(2), 'gs', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
    
    % Call fmincon to minimize the objective
   [thetaSolution, fval, exitflag, output] = fmincon(...
        objectiveFunction, ...
        initialGuess, ...
        [], [], ... % Linear inequality constraints
        [], [], ... % Linear equality constraints
        [], [], ... % Bounds
        [], ... % Nonlinear constraints
        options);

    % Display the solution
    disp('Joint angles solution (in radians):');
    disp(thetaSolution);

    function stop = outfun(theta, optimValues, state)
        stop = false;
        switch state
            case 'iter'
                % Update the plot with the current robot arm position
                plot_robot_arm(hAx, l1, l2, l3, theta, axisLimits,desiredPosition);
                drawnow;
                pause(0.5); % Pause for half a second
        end
    end
end

function plot_robot_arm(ax, l1, l2, l3, theta, axisLimits,desiredPosition)
    % Calculate joint positions
    p0 = [0; 0];
    p1 = p0 + l1 * [cos(theta(1)); sin(theta(1))];
    p2 = p1 + l2 * [cos(theta(1) + theta(2)); sin(theta(1) + theta(2))];
    p3 = p2 + l3 * [cos(theta(1) + theta(2) + theta(3)); sin(theta(1) + theta(2) + theta(3))];

    % Clear existing robot arm plot
    cla(ax);

    % Plot the robot arm
    plot(ax, [p0(1), p1(1), p2(1), p3(1)], [p0(2), p1(2), p2(2), p3(2)], ...
         'bo-', 'MarkerFaceColor', 'b');
    
    % Re-plot the desired position as a green square
    plot(ax, desiredPosition(1), desiredPosition(2), 'gs', 'MarkerSize', 10, 'MarkerFaceColor', 'g');

    % Set the axes limits to the specified values
    xlim(ax, axisLimits(1:2));
    ylim(ax, axisLimits(3:4));
end