function [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y)
%
% Task: returns the two possible solutions of q1 q2 coordinate in the joint space
%       from a given point in the x y cartesian space
%       using geometry rules since we know that we have two links only of length L1 and L2
%       
%
% Inputs:
%-L1: length of the first link of the 2d planar 2 link robot arm
%-L2: length of the second link of the 2d planar 2 link robot arm
%-x: x coordinate of the given point
%-y: x coordinate of the given point
%
% Outputs:
%-q1: q1 coordinate of the given point (first solution)
%-q2: q2 coordinate of the given point (first solution)
%-q1_: q1 coordinate of the given point (second solution)
%-q2_: q2 coordinate of the given point (second solution)
%		
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
  q1 = acosd(x/sqrt(x^2+y^2)) - acosd( (-L2^2+x^2+y^2+L1^2) / (2*L1*sqrt(x^2+y^2)));
  q2 = 180 - acosd( (L1^2+L2^2-(x^2+y^2)) / (2*L1*L2));
  
  q1_ = q1 + 2*acosd( (-L2^2+x^2+y^2+L1^2) / (2*L1*sqrt(x^2+y^2)));
  q2_ = -q2;
endfunction