function [Q,IsVal] = InvKin6dof_stance(XYZ_target,L,wleg)
% Input parameters:  XYZ_target relative to the Sr
%                    L, link lenghts
%                    wleg, which leg -> +1 is right; -1 is left
% Output parameters: Q, joint vector (6x1)
%                    IsVal, valide/possible solution
%--------------------------------------------------------------------------
% Constants & variable atribution -----------------------------------------
  l0 = L(1); l1 = L(2); l2 = L(3); l21 = L(4); l22 = L(5); l3 = L(6);
  xh = XYZ_target(1)
  yh = XYZ_target(2)-wleg*l3/2
  zh = XYZ_target(3) % HIP -------
  %xh, yh, zh
% Ankle joint (lateral plane) ---------------------------------------------
  q1 = wleg*atan2(-wleg*yh,zh-l0)
  
% Avoid particular cases: s1 = 0 or c1 = 0 --------------------------------  
  if (q1 > pi/4 && q1 < 3*pi/4) || (q1 < -pi/4 && q1 > -3*pi/4)
      A = yh/sin(q1);
  else
      A = -(zh-l0)/cos(q1);
  end
% Ankle joint (sagittal plane)---------------------------------------------
  k1 =  wleg*2*l1*xh;    
  k2 = -2*l1*A;
  k3 = xh^2+A^2+l1^2-l2^2;
  AUX_var = k1^2+k2^2-k3^2;
  if abs(AUX_var) < 1e-6,
     AUX_var = 0;
  elseif AUX_var < 0,
     disp('ERROR MSG:InvKin6dof:q2')
     Q = NaN; IsVal = 0; return 
  end
  
  q2 = atan2(k1,k2)+wleg*atan2(sqrt(AUX_var),k3)
% Angkle in the interval -pi to pi ----------------------------------------
  if q2 >  pi,    q2 = q2-2*pi;   end
  if q2 < -pi,    q2 = q2+2*pi;   end
% Knee joint --------------------------------------------------------------
  k2 = 2*l1*l2;
  k3 = xh^2+A^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('ERROR MSG:InvKin6dof:q3')
     Q = NaN; IsVal = 0; return 
  end
  q3 = -wleg*atan2(sqrt(AUX_var),k3);
% Hip joint (lateral plane)------------------------------------------------
  q5 = asin(-sin(q1)*cos(q2+q3));
% Hip joint (vertical axis)------------------------------------------------
 % q4 = acos(cos(q1)/cos(q5));
  k1 = cos(q1)*sin(q2+q3)*cos(q5);
  k2 = wleg*sin(q1)*cos(q5);
  k3 = -wleg*cos(q1)*cos(q2+q3)*sin(q5);
  AUX_var = k1^2+k2^2-k3^2;
  if abs(AUX_var) < 1e-6,
     AUX_var = 0;
  elseif AUX_var < 0,
     disp('ERROR MSG:InvKin6dof:q4')
     Q = NaN; IsVal = 0; return 
  end
  q4 = atan2(k1,k2)+wleg*atan2(sqrt(AUX_var),k3);
% Angkle in the interval -pi to pi ----------------------------------------
  if q4 >  pi,    q4 = q4-2*pi;   end
  if q4 < -pi,    q4 = q4+2*pi;   end
% Hip joint (sagittal plane) -----------------------------------------------
 % q6 = acos(cos(q2+q3)*cos(q4));
  k1 = ( -wleg*cos(q2+q3)*sin(q4)*sin(q5)-sin(q2+q3)*cos(q5) );
  k2 = cos(q2+q3)*cos(q4);
  q6 = atan2(k1,k2);
% Angkle in the interval -pi to pi ----------------------------------------
  if q6 >  pi,    q6 = q6-2*pi;   end
  if q6 < -pi,    q6 = q6+2*pi;   end
% Joint vector Q ----------------------------------------------------------
  Q = [ q1 q2 q3 q4 q5 q6]';
  IsVal = 1;
%-----------------------------------------------------------------------end

