uint8_t BipedController::GetSingleKinematicAction(double x, double y, double z, double heading, uint8_t efector, Action *action){
	double Afx, Afz, SideB, SideA, Hypotenuse, Alfa, Beta, Gamma, AngThigh, AngShin, AngAnkle; 
	uint8_t errcode = 0;

	Afx=cos(heading)*L2;
	SideB = x - Afx;
	Afz = sin(heading)*L2;
	SideA = z - Afz;
	Hypotenuse = sqrt((SideA*SideA)+(SideB*SideB));
	Alfa=atan2(SideA,SideB);
	
	Beta = acos(RangeAbs1(((L1*L1)-(L1*L1)+(Hypotenuse*Hypotenuse))/(2*L1*Hypotenuse)));
	AngThigh = Alfa + Beta; 
	Gamma = acos(RangeAbs1(((L1*L1)+(L1*L1)-(Hypotenuse*Hypotenuse))/(2*L1*L1)));
	AngShin = -(RAD(180)-Gamma); 
	AngAnkle = -heading - AngThigh - AngShin; 
	AngAnkle = -(RAD(180)-AngAnkle);
	// Result in degrees
	Alfa = DEG(AngThigh); // hip pitch
	Beta = DEG(AngShin); // knee pitch
	Gamma = DEG(AngAnkle);// ankle pitch
	
	// Check angle out of range
	if(Alfa < MIN_HIP_DEG){
		Alfa = MIN_HIP_DEG;
		errcode |= 0x01;
	}
	if(Alfa > MAX_HIP_DEG){
		Alfa = MAX_HIP_DEG;
		errcode |= 0x02;
	}
	if(Beta < MIN_KNEE_DEG){
		Beta = MIN_KNEE_DEG;
		errcode |= 0x04;
	}
	if(Beta > MAX_KNEE_DEG){
		Beta = MAX_KNEE_DEG;
		errcode |= 0x08;
	}
	if(Gamma < MIN_ANKLE_DEG){
		Gamma = MIN_ANKLE_DEG;
		errcode |= 0x10;
	}
	if(Gamma > MAX_ANKLE_DEG){
		Gamma = MAX_ANKLE_DEG;
		errcode |= 0x20;
	}
	
	// Calculate action to execute
	switch(efector){
		case LEFT_EFFECTOR:
			this->leftFoot->x = x;
			this->leftFoot->y = y;	
			this->leftFoot->z = z;
			this->leftFoot->heading = heading;
			action->leftAnkleRoll = LEFT_FOOT_EQUATION(y);
			action->leftAnklePitch = LEFT_ANKLE_EQUATION(Gamma);
			action->leftKneePitch = LEFT_KNEE_EQUATION(Beta);
			action->leftHipPitch = LEFT_HIP_EQUATION(Alfa);	
			action->leftHipRoll = LEFT_ROTHIP_EQUATION(y);
			break;
		case RIGHT_EFFECTOR:
			this->rightFoot->x = x;
			this->rightFoot->y = y;	
			this->rightFoot->z = z;
			this->rightFoot->heading = heading;
			action->rightAnklePitch = RIGHT_ANKLE_EQUATION(Gamma);
			action->rightAnkleRoll = RIGHT_FOOT_EQUATION(y);
			action->rightKneePitch = RIGHT_KNEE_EQUATION(Beta);
			action->rightHipPitch = RIGHT_HIP_EQUATION(Alfa);	
			action->rightHipRoll = RIGHT_ROTHIP_EQUATION(y);
			break;
	}
	return errcode;
}