function [ANCA JOELHO TORNOZELO]=PC_Inv_Kin_Perna_Livre(x,y,phi)
%% Funcao para cinemática inversa da perna livre do humanoide
% INPUT
% x - posição horizontal relativamente à cota do tornozelo
% y - posição vertical relativamente à base do pé
% phi - orientação do pé em relação ao eixo vertical
% OUTPUT
% ANCA - valor do ângulo para a flexão da anca
% JOELHO - valor do ângulo para a flexão do joelho
% TORNOZELO - valor do ângulo para a flexão do tornozelo

cmp_perna=50+139+143; %mm

L1=143; %mm
L2=139; %mm
L3=50; %mm

pwy=x-L3*sind(phi);
pwx=cmp_perna-y-L3*cosd(phi);

Q=invkinRR( pwx, pwy, L1, L2 );
if isempty(Q)
    ANCA=[];
    JOELHO=[];
    TORNOZELO=[];
    return
end
Q(3,:)=zeros(1,size(Q,2));
Q(3,1)=phi-Q(1,1)-Q(2,1);
Q(3,2)=phi-Q(1,2)-Q(2,2);

% verificacao limites junta  do joelho
if Q(2,1)>0 || Q(2,1)<-130
    Q(:,1)=[];
elseif Q(2,2)>0 || Q(2,2)<-130
    Q(:,2)=[];
end

% verificar valores tornozelo
if  Q(3,1)>=-45 && Q(3,1)<=20
    % verificacao limites junta  do joelho
    if Q(2,1)>0 || Q(2,1)<-130
        Q(:,1)=[];
    end
    ANCA=Q(1,:);
    JOELHO=Q(2,:);
    TORNOZELO=Q(3,:);
elseif Q(3,1)>20 && phi==0
    % casos em que theta_tornozelo>20 e phi=0
    Ln=sqrt((L3*sind(20))^2+(L2+L3*cosd(20))^2);
    Qn=invkinRR( cmp_perna-y, x, L1, Ln );
    if isempty(Qn)
        ANCA=[];
        JOELHO=[];
        TORNOZELO=[];
        return
    end
    Q=Qn;
    Q(3,:)=20*ones(1,size(Q,2));
    % verificacao limites junta  do joelho
    i_jnt=2;
    if Q(2,1)>0 || Q(2,1)<-130
        Q(:,1)=[];
        i_jnt=1;
    end
    if Q(2,i_jnt)>0 || Q(2,i_jnt)<-130
        Q(:,i_jnt)=[];
        if i_jnt==1
            ANCA=[];
            JOELHO=[];
            TORNOZELO=[];
            return
        end
    end
    
    ANCA=Q(1,:);
    JOELHO=Q(2,:);
    TORNOZELO=Q(3,:);
elseif Q(3,1)<-45 && phi==0
    % casos em que theta_tornozelo<-45 e phi=0
    Ln=sqrt((L3*sind(45))^2+(L2+L3*cosd(45))^2);
    Qn=invkinRR( cmp_perna-y, x, L1, Ln );
    if isempty(Qn)
        ANCA=[];
        JOELHO=[];
        TORNOZELO=[];
        return
    end
    Q=Qn;
    Q(3,:)=-45*ones(1,size(Q,2));
    % verificacao limites junta  do joelho
    i_jnt=2;
    if Q(2,1)>0 || Q(2,1)<-130
        Q(:,1)=[];
        i_jnt=1;
    end
    if Q(2,i_jnt)>0 || Q(2,i_jnt)<-130
        Q(:,i_jnt)=[];
        if i_jnt==1
            ANCA=[];
            JOELHO=[];
            TORNOZELO=[];
            return
        end
    end
    
    ANCA=Q(1,:);
    JOELHO=Q(2,:);
    TORNOZELO=Q(3,:);
else
    ANCA=[];
    JOELHO=[];
    TORNOZELO=[];
    return
end

if isempty(ANCA)
    ANCA=[];
    JOELHO=[];
    TORNOZELO=[];
    return
end
% verificar junta anca
if ANCA<-30 || ANCA>120
    ANCA=[];
    JOELHO=[];
    TORNOZELO=[];
    return
end

% verificar somatório dos angulos = phi
if ANCA+JOELHO+TORNOZELO ~= phi
    ANCA=[];
    JOELHO=[];
    TORNOZELO=[];
end
end