function [ANCA JOELHO TORNOZELO]=PC_Inv_Kin_Perna_Apoio(x,y,phi)
%% Funcao para cinemática inversa da perna de apoio do humanoide
% INPUT
% x - posição horizontal relativamente ao eixo da anca
% y - posição vertical relativamente ao eixo da anca
% phi - orientação da pélvis 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

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

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

Q=invkinRR( cmp_perna+y, x, L1, L2 );
if isempty(Q)
    ANCA=[];
    JOELHO=[];
    TORNOZELO=[];
    return
end

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


% verificar angulo tornozelo
if Q(1)>20
    x_calc=x-L1*sind(20);
    y_calc=cmp_perna+y-L1*cosd(20);
    if sqrt((x_calc)^2+(y_calc)^2)~=L2
        ANCA=[];
        JOELHO=[];
        TORNOZELO=[];
        return
    end
    Q(2)=-(radtodeg(atan2(y_calc,x_calc))-20);
    Q(1)=20;
elseif Q(1)<-45
    x_calc=x-L1*sind(-45);
    y_calc=cmp_perna+y-L1*cosd(-45);
    if sqrt((x_calc)^2+(y_calc)^2)~=L2
        ANCA=[];
        JOELHO=[];
        TORNOZELO=[];
        return
    end
    Q(2)=-(radtodeg(atan2(y_calc,x_calc))-20);
    Q(1)=20;
end

% corrigir phi
ang_vertical=Q(1)+Q(2);
Q(3)=-ang_vertical+phi;

% verificar limite junta anca
if Q(3)>120 || Q(3)<-30
    ANCA=[];
    JOELHO=[];
    TORNOZELO=[];
    return
else
    ANCA=Q(3);
    JOELHO=Q(2);
    TORNOZELO=Q(1);
end