function [teta1, teta2] = inv_cine(x,y,l1, l2, c)

%INV_CINE   Cinemática inversa 2R
%   [teta1, teta2] = inv_cine(x,y,l1,l2,c), devolve a posição angular 
%   das juntas no sentido de atingir a posição (x,y).
%   PARAMETROS DE ENTRADA:
%       x é a abcissa da posição pretendida
%       y é a ordenada da posição pretendida
%       l1 é l2 é o comprimento do braço respectivo
%       c é a indicação do cotovelo para cima ou para baixo
%   ------------------------
%   PARAMETROS DE SAIDA:
%       teta1 é o angulo da junta 1 em relação à referencia
%       teta2 é o angulo da junta 2 em relação ao junta 1


    % para teta1
    k1 = 2.*y*l1;
    k2 = 2.*x*l1;
    k3 = x.^2+y.^2+l1.^2-l2.^2;
    
    teta1 = atan2(k1,k2)+c*atan2(sqrt(k1.^2+k2.^2-k3.^2),k3)-pi/2;

    
    % para teta2
    k1 = 0;
    k2 = 2*l1*l2;
    k3 = x.^2+y.^2-l1^2-l2^2;

    teta2 = -(atan2(k1,k2)-c*atan2(sqrt(k1.^2+k2.^2-k3.^2),k3));