function theta=kininv2R(p,phi,a1,a2,a3)

% calcola le due soluzioni gomito UP e gomito DOWN
% della cinematica inversa del robot planare 3R
% Descrizione input
% p= [px; py] : coordinate cartesiane
% phi : angolo di orientamento mano
% a1, a2 e a3 : lunghezze dei bracci
% Descrizione Output
% theta(:,1) soluzione DOWN:  theta(2,1)>0
% theta(:,2) soluzione UP:    theta(2,2)=-theta(2,1)

px=p(1); py=p(2);

theta=zeros(3,2);

c=(px^2+py^2-a1^2-a2^2)/(2*a1*a2);

if abs(c)>1, disp('WARNING: out of reach'); return, end,
if c==1, theta(1,:)=atan2(py,px)*[1 1]; return, end,  % BRACCIO STESO: theta_2=0 e SOLUZIONI COINCIDENTI
if c==-1
    theta(2,:)=pi*[1 -1];   % BRACCIO RIPIEGATO SU SE STESSO
    if px*py==0, return,    % BRACCIO RIPIEGATO NELL'ORIGINE
    else
        r=a1-a2;   % SOLUZIONI DIVERSE SE a1 > a2 o viceversa
        theta(1,:)=atan2(r*py,r*px)*[1 1];
    end
    return
end
if abs(c)<1
    s=sqrt(1-c^2);
    theta2=atan2(s,c);
    theta(2,:)=[theta2 -theta2];
    R2=[c -s; s c]; I2=eye(2);
    b=[px; py];
    A=a1*I2+a2*R2;
    z=A\b;
    theta(1,1)=atan2(z(2),z(1));
    A=a1*I2+a2*R2';
    z=A\b;
    theta(1,2)=atan2(z(2),z(1));
end
    
    
    

        
    

