function [Q,IsVal] = InvKin_4dof(qi,q_lim,P_ee,L,warm)
%__________________________________________________________________________
% 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  
 lt = L(1); lt_l = L(2); lt_w = L(3); lt_h = L(4); 
 lr = L(5); l0 = L(6); 
 l1 = L(7); l1_1 = L(8); l1_2 = L(9); l2 = L(10);
 xe = P_ee(1); ye = P_ee(2); ze = P_ee(3);
 q1i = qi(1); q2i = qi(2); q3i = qi(3); q4i = qi(4);
% Decodes the desired arm
 if warm == +1, FLAG_sign =  1;     %Left arm
 else           FLAG_sign = -1; end % Right arm     
% Joint#4: elbow joint 
 k2=2*l1*l2;
 k3=xe^2+(ye-FLAG_sign*(lt_l/2+l0))^2+(ze-(lt+lt_h-lr))^2-l2^2-l1^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=FLAG_sign*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
% Eliminates the redundancy -----------------------------------------------
if (warm == +1 && ye >= (lt_l/2+l0)) || (warm == -1 && ye <= -(lt_l/2+l0)),
   % Joint#3: vertical axis
   q3 = 0;
   % Joint#1: sagital shoulder joint
   k1 = (ze-(lt+lt_h-lr));
   k2 = FLAG_sign*xe;
   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: lateral shoulder joint
   k2 = (l1+l2*cos(q4));
   k3 = FLAG_sign*xe*sin(q1)-( ze-(lt+lt_h-lr) )*cos(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
   q2 = FLAG_sign*atan2(sqrt(AUX_var),k3);
else % Search for the optimal configuration -------------------------------
    % Joint#3: vertical axis
    if warm == +1; q3_min = 0;     q3_max = pi/2;
    else           q3_min = -pi/2; q3_max = 0; 
    end
    while q3_max-q3_min > 2*pi/180, % resolution: 2º
          % Joint#3: vertical axis
          q3 = (q3_max+q3_min)/2;
          % Joint#1: sagital shoulder joint
          k1 = (ze-(lt+lt_h-lr));
          k2 = FLAG_sign*xe;
          k3 = l2*cos(q3)*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
          % Verifies the joint limits
          if ( warm == +1 && (q1 <  q_lim(1,1) || q1 >  q_lim(2,1)) ) ||... 
             ( warm == -1 && (q1 < -q_lim(2,1) || q1 > -q_lim(1,1)) ),
             if warm == +1; q3_min = q3; else q3_max = q3; end
             disp('ola')
             continue
          end
          % Joint#2: lateral shoulder joint
          k1 = l2*sin(q3)*sin(q4);
          k2 = FLAG_sign*(l1+l2*cos(q4));
          k3 = xe*sin(q1)-FLAG_sign*( ze-(lt+lt_h-lr) )*cos(q1);
          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
          if ye <= FLAG_sign*(lt_l/2+l0),
             q2 = atan2(k1,k2)-FLAG_sign*atan2(sqrt(AUX_var),k3);
          else
              q2 = atan2(k1,k2)+FLAG_sign*atan2(sqrt(AUX_var),k3);
          end
          if q2 >  pi, q2 = q2-2*pi;   end
          if q2 < -pi, q2 = q2+2*pi;   end
          %Q = [q1,q2,q3 ]*180/pi
          % Verifies the joint limits
          if ( warm == +1 && (q2 <  q_lim(1,2) || q2 >  q_lim(2,2)) ) ||... 
             ( warm == -1 && (q2 < -q_lim(2,2) || q2 > -q_lim(1,2)) ),
             if warm == +1; q3_min = q3; else q3_max = q3; end             
             continue
          else
              if warm == +1; q3_max = q3; else q3_min = q3; end
              continue
          end
    end    
end
% [q1 q2 q3 q4]*180/pi
% Joint angles matrix  
 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
%__________________________________________________________________________
end