function q = InvKin_3dof(xe,ye,ze,l0,l1,l2)

q0 = atan2(-ye,ze-l0);

k1 = 2*l1*xe;
if abs(q0) > pi /4 && abs(q0) < 3*pi/4, 
   k2 = -2*l1*ye/sin(q0);
   k3 = xe^2+(ye/sin(q0))^2+l1^2-l2^2;
else
    k2 = 2*l1*(ze-l0)/cos(q0);
    k3 = xe^2+((ze-l0)/cos(q0))^2+l1^2-l2^2;
end

q1 = atan2(k1,k2)+atan2(sqrt(k1^2+k2^2-k3^2),k3);
if q1 >  pi, q1 = q1-2*pi; end
if q1 < -pi, q1 = q1+2*pi; end
    
k1 = 0;
k2 = 2*l1*l2;
if abs(q0) > pi/4 && abs(q0) < 3*pi/4, 
   k3 = xe^2+(ye/sin(q0))^2-l1^2-l2^2;
else
    k3 = xe^2+((ze-l0)/cos(q0))^2-l1^2-l2^2;
end
q2 = -atan2(sqrt(k1^2+k2^2-k3^2),k3);

q = [q0 q1 q2]*180/pi;
% q(1) = q(1) * 32/8;
% q(2) = q(2) * 32/8;
% q(3) = (-q(3) * 30/22)-90;