function [Q,IsVal] = InvKin_4dof(qi,P_ee,L,warm)
%__________________________________________________________________________
% in_args: P_ee, tip cartesian coordinates 
%          q_lim, physical limits of the individual joints
%          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); %+/-
% Joint#3: vertical axis
 q3 = -pi/2:pi/180:+pi/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
% 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
 for tt = 1:length(q3),
     if ye <= FLAG_sign*(lt_l/2+l0),
        q2(tt) = atan2(k1(tt),k2)-FLAG_sign*atan2(sqrt(AUX_var(tt)),k3(tt));
     else
         q2(tt) = atan2(k1(tt),k2)+FLAG_sign*atan2(sqrt(AUX_var(tt)),k3(tt));
     end
 end
 if q2 >  pi, q2 = q2-2*pi;   end
 if q2 < -pi, q2 = q2+2*pi;   end
 
% Plot time courses 
% figure
%  subplot(4,1,1), plot(q3*180/pi,q1*180/pi,'r',[-90 90],[q1i q1i],'k.-'), ylabel('q1')
%  subplot(4,1,2), plot(q3*180/pi,q2*180/pi,'r',[-90 90],[q2i q2i],'k.-'), ylabel('q2')
%  subplot(4,1,3), plot(q3*180/pi,q3*180/pi,'r',[-90 90],[q3i q3i],'k.-'), ylabel('q3')
%  subplot(4,1,4), plot(q3*180/pi,q4*180/pi,'r',[-90 90],[q4i q4i],'k.-'), ylabel('q4')

% Computes minimal total angular displacement
 if warm == +1, ILog = q2 >= 0;
 else           ILog = q2 <= 0; end
 dq1 = abs(q1-q1i); dq2 = abs(q2-q2i); dq3 = abs(q3-q3i);
 dq = dq1+dq2+dq3;
 min_var = min(dq(ILog));
 I_min = find(dq == min_var);
 %AUX = [min_var*180/pi I_min(1)]
 %Q_sol = [q1(I_min(1)) q2(I_min(1)) q3(I_min(1)) q4(I_min(1))]*180/pi
% Joint angles matrix 
 Q = [q1(I_min(1)) q2(I_min(1)) q3(I_min(1)) q4]';
 IsVal = 1;
%__________________________________________________________________________
end

% % Joint#2: lateral shoulder motion 
%  k2=(l1+l2*cos(q4);
%  k3=sin(q1)*xe-cos(q1)*ze;
%  AUX_var = k2^2-k3^2;
%  if AUX_var < 0,
%      disp('OUT of WorkVolume: input another desired tip position...')
%      Q = NaN; ILog = 0; return 
%  end
%  q2=atan2(sqrt(AUX_var),k3); %+/-
%  q2_sol = [q2 -q2]; 
%  
% % Joint#1: vertical joint
%  q1=atan2(ye,xe);
%  if q1 >= 0 & q1 <= pi,
%     q1_sol = [q1 q1 q1-pi q1-pi];
%  else
%      q1_sol = [q1 q1 q1+pi q1+pi];
%  end 
% % Defines auxiliar constants A and B
%  if abs(q1) <= pi/4 | abs(q1) > 3*pi/4, 
%     A=-xe/cos(q1)+l4*cos(q_tool);
%  else 
%      A=-ye/sin(q1)+l4*cos(q_tool);
%  end
%  B=ze-d1-l4*sin(q_tool);
%  %A, B, pause
% % Joint#2: shoulder
%  k1=2*l2*A;
%  k2=2*l2*B;
%  k3=A^2+B^2+l2^2-l3^2;
%  AUX_var = k1^2+k2^2-k3^2;
%  if AUX_var < 0,
%      disp('OUT of WorkVolume: input another desired tip position...')
%      Q = NaN; ILog = 0; return 
%  end
%  q2_1=atan2(k1,k2)-atan2(sqrt(AUX_var),k3);
%  q2_2=atan2(k1,k2)+atan2(sqrt(AUX_var),k3);
%  q2_sol = [q2_1 q2_2 -q2_1 -q2_2 ];
%  for kk = 1:4,
%      if q2_sol(kk) >  pi, q2_sol(kk) = q2_sol(kk)-2*pi;   end
%      if q2_sol(kk) < -pi, q2_sol(kk) = q2_sol(kk)+2*pi;   end
%  end
% % Joint#3: elbow
%  k2=2*l2*l3;
%  k3=A^2+B^2-l3^2-l2^2;
%  AUX_var = k2^2-k3^2;
%  if AUX_var < 0,
%      disp('OUT of WorkVolume: input another desired tip position...')
%      Q = NaN; ILog = 0; return 
%  end
%  q3=atan2(sqrt(AUX_var),k3); %+/-
%  q3_sol = [q3 -q3 -q3  q3];
% % Joint#4: wrist
%  q4_sol=q_tool-pi/2-q2_sol(1:2)-q3_sol(1:2);
%  q4_sol(3) = -q4_sol(1);
%  q4_sol(4) = -q4_sol(2);
% % Angles between -pi and pi
%  for kk = 1:4,
%      if q4_sol(kk) >  pi, q4_sol(kk) = q4_sol(kk)-2*pi;   end
%      if q4_sol(kk) < -pi, q4_sol(kk) = q4_sol(kk)+2*pi;   end
%  end
% % Defines which of the 4-solutions are suitable
%  if size(q_lim,1) == 1, q_lim = q_lim'; end % row vector
%  ILog_lim1 = abs(q1_sol) <= q_lim(1);    % Physical limits: q1 
%  ILog_lim2 = abs(q2_sol) <= q_lim(2);    % Physical limits: q2
%  ILog_lim3 = abs(q3_sol) <= q_lim(3);    % Physical limits: q3
%  ILog_lim4 = abs(q4_sol) <= q_lim(4);    % Physical limits: q4
%  ILog_lim = ILog_lim1 & ILog_lim2 & ILog_lim3 & ILog_lim4; 
%  ELBOW_conf = ['d' 'u' 'd' 'u'];    
%  ILog_elbow = ELBOW_conf == ELBOW;       % Desired elbow configuration
%  ILog = ILog_lim & ILog_elbow;
% Joint angles matrix 
%  Q = [q1_sol; q2_sol; q3_sol; q4_sol];
%    ANG1 = q1_sol*180/pi
%    ANG2 = q2_sol*180/pi
%    ANG3 = q3_sol*180/pi
%    ANG4 = q4_sol*180/pi
%__________________________________________________________________________
% end
