function [th1,th2] = xy2ang(x,y)

thoff = 0; % pi/2;  % Offset is angle of th1 when encoder is initialized

R = (x.^2+y.^2).^.5;
A = atan2(y,x);

L1 = 178;  % mm
L2 = 88; % mm

% Use the Law of Cosines to get inter-arm angle:
gamma = acos((-R.^2 + L1.^2 + L2.^2)./(2*L1.*L2));
alpha = acos((-L2.^2 + R.^2 + L1.^2)./(2*L1.*R));

th1 = A - alpha + thoff;  % pi/2 offset to make it wrt to "straight out"
th2 = pi - gamma;
