function theta = inverseKinematics(x, y, L1, L2)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculate inverse kinematics for a two-link planar robot
% Inputs:
%   - x, y: Cartesian coordinates
%   - L1, L2: Link lengths
% Output:
%   - theta: Joint angles [q1; q2]
% author: Camille CONJAT, camille.conjat@ecam.fr
% date: 03/12/2023
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calculate the term inside the acos function
term = (x^2 + y^2 - L1^2 - L2^2) / (2 * L1 * L2);

% Check for solutions
if term > 1 || term < -1
  % No solution
  disp('No solution exists for the given coordinates.');
  theta = [];
  return;
elseif term == 1
  % One solution
  q1 = atan2(y, x);
  q2 = 0;
elseif term == -1
  % One solution
  q1 = atan2(y, x);
  q2 = pi;
else
  % Two solutions
  q1_1 = atan2(y, x) - atan2(L2 * sin(acos(term)), L1 + L2 * cos(acos(term)));
  q2_1 = acos(term);

  q1_2 = atan2(y, x) + atan2(L2 * sin(acos(term)), L1 + L2 * cos(acos(term)));
  q2_2 = -acos(term);

  % Choose the solutions based on the robot's configuration
  q1 = [q1_1; q1_2];
  q2 = [q2_1; q2_2];
end

% Convert angles to degrees
theta = rad2deg([q1; q2]);
end