function [Q,IsVal] = InvKin_6dof(P_ee)
%__________________________________________________________________________
% in_args: qi, actual joint angular position
%          q_lim, physical limits of the individual joints
%          P_ee, tip cartesian coordinates 
%          L, arm's link lengths
%          warm, which arm: left +1 or right -1
% out_args: Q, joint matrix Q(4jointx4solutions)
%           IsVal, valide/possible solution
%
% Decodes input arguments
 l1= 146; l1_1 = 81; l1_2 = 65; l2= 140; l3 = 47; % Link lenght
 % Global variables (only animation purposes!) -----------------------------
 global Hh P_trace
 Hh = (l1+l2+l3)*0.923; 
 
Gain(1)	= (30/22);
Gain(2)	= (32/8);
Gain(3)	= 1;
Gain(4)	= (30/22);
Gain(5)	= (32/8);
Gain(6)	= (32/8);

% Coordinates transformation 
 global Hh
 xe = Hh-P_ee(3)-l3; ye = P_ee(1); ze = -P_ee(2);
% xe, ye, ze, pause
% Joint#4: KNEE 
 k2=2*l1*l2;
 k3=xe^2+ye^2+ze^2-l1^2-l2^2;
 AUX_var = k2^2-k3^2;
 if abs(AUX_var) < 1e-6,
    AUX_var = 0;
 elseif AUX_var < 0,
     disp('OUT4 of WorkVolume: input another desired tip position...')
     Q = NaN; IsVal = 0; return 
 end
 q4=-atan2(sqrt(AUX_var),k3); %+/-
% Verifies the joint limits
%  if ( warm == +1 && (q4 <  q_lim(1,4) || q4 >  q_lim(2,4)) ) || ... 
%     ( warm == -1 && (q4 < -q_lim(2,4) || q4 > -q_lim(1,4)) ),
%     Q = [];
%     IsVal = 0;
%     return
%  end
% Joint#3: HIP (vertical)
 q3 = 0;
% Joint#1: HIP (saggital)
 k1 = xe;
 k2 = -ye;
 k3 = -l2*sin(q4);
 AUX_var = k1^2+k2^2-k3^2;
 if abs(AUX_var) < 1e-6,
    AUX_var = 0;
 elseif AUX_var < 0,
     disp('OUT1 of WorkVolume: input another desired tip position...')
     Q = NaN; IsVal = 0; return 
 end
 q1 = atan2(k1,k2)-atan2(sqrt(AUX_var),k3);
 if q1 >  pi, q1 = q1-2*pi;   end
 if q1 < -pi, q1 = q1+2*pi;   end 
% Joint#2: HIP (lateral)
 k2 = (l1+l2*cos(q4));
 k3 = xe*cos(q1)+ye*sin(q1);
 AUX_var = k2^2-k3^2;
 if abs(AUX_var) < 1e-6,
    AUX_var = 0;
 elseif AUX_var < 0,
     disp('OUT2 of WorkVolume: input another desired tip position...')
     Q = NaN; IsVal = 0; return 
 end
 if ze >= 0,
    q2 = -atan2(sqrt(AUX_var),k3);    
 else
     q2 = atan2(sqrt(AUX_var),k3);
 end
% Joint#6: ANKLE (lateral)
 q6 = asin(-cos(q1)*sin(q2));
% Joint#5: ANKLE (saggital)
 q5 = -q4-asin(sin(q1)/cos(q6));
% [q1 q2 q3 q4 q5 q6]*180/pi
 Q = [q1 q2 q3 q4 q5 q6]';
 IsVal = 1;
%  if warm == +1 && (q1 >=  q_lim(1,1) && q1 <=  q_lim(2,1)) && ...
%                   (q2 >=  q_lim(1,2) && q2 <=  q_lim(2,2)) && ...
%                   (q3 >=  q_lim(1,3) && q3 <=  q_lim(2,3)) && ...
%                   (q4 >=  q_lim(1,4) && q4 <=  q_lim(2,4)) || ...
%     warm == -1 && (q1 >= -q_lim(2,1) && q1 <= -q_lim(1,1)) && ...
%                   (q2 >= -q_lim(2,2) && q2 <= -q_lim(1,2)) && ...
%                   (q3 >= -q_lim(2,3) && q3 <= -q_lim(1,3)) && ...
%                   (q4 >= -q_lim(2,4) && q4 <= -q_lim(1,4)), 
%     Q = [q1 q2 q3 q4]';
%     IsVal = 1;
%  else
%      Q = [];
%      IsVal = 0; 
%  end
%__________________________________________________________________________

% converter e regular
Q = Q*180/pi;
for i=1:6
    Q(i) = Q(i)*Gain(i);
end
Q(1) = -Q(1)+0;
Q(2) = -Q(2)-10;
Q(3) = Q(3)+0;
Q(4) = Q(4)+80;
Q(5) = -Q(5)-10;
Q(6) = Q(6)+0;

end