void SpecificWorker::saccadic4D(float leftPan, float rightPan, float tilt, float neck){
	RoboCompJointMotor::MotorGoalPositionList list;
	RoboCompJointMotor::MotorGoalPosition g;
	g.maxSpeed = 0;
	g.name = leftPanMotorName;
	g.position = leftPan;
	list.push_back( g );
	g.maxSpeed = 0;
	g.name = rightPanMotorName;
	g.position = rightPan;
	list.push_back( g );
	g.maxSpeed = 0;
	g.name = tiltMotorName;
	g.position = tilt;
	list.push_back( g );
	g.maxSpeed = 0;
	g.name = neckMotorName;
	g.position = neck;
	list.push_back( g );
	QMutexLocker lock(mutex);	
	try
	{
		jointmotor1_proxy->setSyncPosition( list );
	}
	catch( RoboCompJointMotor::UnknownMotorException & ex)
	{
		std::cout << ex;
		throw ex;
	}
	catch( RoboCompJointMotor::HardwareFailedException & ex)
	{
		std::cout << ex;
		throw ex;
	}
}
void SpecificWorker::moverangles(QVec angles)
{
	RoboCompJointMotor::MotorGoalPositionList mg;
	RoboCompJointMotor::MotorGoalPosition p;
	float 	q1=angles(0),
			q2=angles(1)*-1,
			q3=angles(2)*-1;
	qDebug()<<"q1 = "<<q1<<"  q2 = "<<q2<<"  q3 = "<<q3;
	MotorState m=jointmotor_proxy->getMotorState(m1);
	
	p.name=m1;
	p.maxSpeed=fabs(q1-m.pos);
	p.position=q1;
	mg.push_back(p);
	
	jointmotor_proxy->getMotorState(m2);
	p.name=m2;
	p.maxSpeed=fabs(q2-m.pos);
	p.position=q2;
	mg.push_back(p);
	
	jointmotor_proxy->getMotorState(m3);
	p.name=m3;
	p.maxSpeed=fabs(q3-m.pos);
	p.position=q3;
	mg.push_back(p);
	jointmotor_proxy->setSyncPosition(mg);
}
void SpecificWorker::moverangles(QVec angles,double vel)
{
	if(!isnan(angles(0))&&!isnan(angles(1))&&!isnan(angles(2)))
	{
		RoboCompJointMotor::MotorGoalPositionList mg;
		RoboCompJointMotor::MotorGoalPosition p;
		RoboCompJointMotor::MotorGoalVelocityList mv;
		RoboCompJointMotor::MotorGoalVelocity v;
		double 	q1=angles(0),
				q2=angles(1) *signleg,
				q3=angles(2) *signleg;
		qDebug()<<"Leg: "<<foot<<"q1 = "<<q1<<"  q2 = "<<q2<<"  q3 = "<<q3;
		MotorState m=jointmotor_proxy->getMotorState(motores.at(0).toStdString());
		v.name = p.name = motores.at(0).toStdString();
		v.velocity = p.maxSpeed=fabs(q1-m.pos)*vel;
		p.position=q1;
		mg.push_back(p);
		mv.push_back(v);
		
		m=jointmotor_proxy->getMotorState(motores.at(1).toStdString());
		v.name = p.name=motores.at(1).toStdString();
		v.velocity = p.maxSpeed=fabs(q2-m.pos)*vel;
		p.position=q2;
		mg.push_back(p);
		mv.push_back(v);
		
		m=jointmotor_proxy->getMotorState(motores.at(2).toStdString());
		v.name = p.name=motores.at(2).toStdString();
		v.velocity = p.maxSpeed=fabs(q3-m.pos)*vel;
		p.position=q3;
		mg.push_back(p);
		mv.push_back(v);
		
// 		jointmotor_proxy->setSyncVelocity(mv);
		jointmotor_proxy->setSyncPosition(mg);
	}
	else
		qDebug()<< "Posicion no alcanzada";

}
///JoystickAdapter
void SpecificWorker::sendData(const TData& data)
{
	qDebug()<<"jdata0"<<data.axes[0].value;
	qDebug()<<"jdata1"<<data.axes[1].value;
	qDebug()<<"jdata2"<<data.axes[2].value;
	if (data.buttons[0].clicked)
	{
		qDebug()<<"button 0 clicked";
		
		
		float joy_y = normalize(data.axes[1].value, -1., 1., -0.6,0.5);
		float joy_x = normalize(data.axes[0].value, -1., 1., -0.5,0.5);
		float rotate = normalize(data.axes[2].value, -1., 1., -0.5,0.5);
		
		qDebug()<<"normalize position 0"<<"pitch"<<joy_x<<"yaw"<<joy_y;
		
		float POS_Z = joy_y;
		float POS_X = -joy_y + joy_x;
		float POS_Y = -joy_y - joy_x;
		
		int step_rotate = rotate * 17960.f / 6.28;
		int pasos_z = POS_Z * 17960.f / 6.28; 
		int pasos_x = POS_X * 17960.f / 6.28;
		int pasos_y = POS_Y * 17960.f / 6.28;
		
		qDebug()<<"pasos"<<pasos_z<<pasos_x<<pasos_y<<step_rotate;
		
		if (data.axes[1].value!=joy_x_antiguo || data.axes[0].value != joy_y_antiguo || data.axes[2].value != joy_z_antiguo)
		{
			
			// limites de los recorridos de los motores
			if ( pasos_x > MIN_X && pasos_x < MAX_X && pasos_y > MIN_Y && pasos_y < MAX_Y && pasos_z > MIN_Z && pasos_z < MAX_Z && step_rotate > MIN_TILT &&  step_rotate < MAX_TILT)
			{
				qDebug()<<"stting positios"<<"front"<<POS_Z<<"right"<<POS_X<<"left"<<POS_Y;
				RoboCompJointMotor::MotorGoalPosition p_goal;
				RoboCompJointMotor::MotorGoalPositionList list;
				
				p_goal.name = "neck";
				p_goal.position = rotate;
				list.push_back(p_goal);
				
				p_goal.name = "neckRight";
				p_goal.position = POS_X;
				list.push_back(p_goal);
				
				p_goal.name = "neckLeft";
				p_goal.position = POS_Y;
				list.push_back(p_goal);
				
				p_goal.name = "neckFront";
				p_goal.position = POS_Z;
				list.push_back(p_goal);
 
				joy_x_antiguo = data.axes[1].value;
				joy_y_antiguo = data.axes[0].value;
				joy_z_antiguo = data.axes[2].value;
				
			
				mutex->lock();
				try
				{
					jointmotor1_proxy->setSyncPosition( list );
				}
				catch( Ice::Exception & ex)
				{
					std::cout<< ex.what()<< std::endl;
				}
				mutex->unlock();
			}
		}
        
	}
	else if (data.buttons[1].clicked)
	{
		// eyebrown and mouth
		qDebug()<<"button 1 clicked";
		
		float axes_y = normalize(data.axes[1].value, -1., 1., -0.6,0.5);
		float axes_x = normalize(data.axes[0].value, -1., 1., -0.5,0.5);
		float axes_mouth = normalize(data.axes[2].value, -1., 1., -0.5,0.5);
		
		qDebug()<<"normalize position 1"<<"pitch"<<axes_x<<"yaw"<<axes_y<<"roll"<<axes_mouth;
		
		move(axes_mouth/100, mouthMotorName);
	
		move(-axes_x/100, eyeBLeftYAWMotorName );
	
		move(axes_x/100, eyeBRightYAWMotorName );

		move(axes_y/100, eyeBLeftUPMotorName );
	
		move(axes_y/100, eyeBRightUPMotorName );
				
	}
	else if (data.buttons[2].clicked)
	{
				qDebug()<<"button 2 clicked";
		float rot_tilt = normalize(data.axes[1].value, -1., 1., -0.5,0.5) /3;
		float rotations_eyeA = normalize(data.axes[0].value, -1., 1., -0.5,0.5) /2;
		
				RoboCompJointMotor::MotorGoalPosition p_goal;
				RoboCompJointMotor::MotorGoalPositionList list;
		
				p_goal.name = "rightPan";
				p_goal.position = rotations_eyeA;
				list.push_back(p_goal);
				
							
				p_goal.name = "leftPan";
				p_goal.position = rotations_eyeA;
				list.push_back(p_goal);
				
				
				p_goal.name = "tilt";
				p_goal.position = rot_tilt;
				list.push_back(p_goal);
			
				mutex->lock();
				try
				{
					jointmotor1_proxy->setSyncPosition( list );
				}
				catch( Ice::Exception & ex)
				{
					std::cout<< ex.what()<< std::endl;
				}
				mutex->unlock();
		
	}
		
}
void SpecificWorker::moveneck()
{
		float rotations_eyeA = righteyepan->value();
		float rotations_eyeB = lefteyepan->value();
		
		float eyestilt = eyetilt->value();
		float ROTATE = neckyaw->value();
		
		float POS_Zz = neckpitch->value();
		float POS_Xx = -neckpitch->value() + neckroll->value();
		float POS_Yy = -neckpitch->value() - neckroll->value();
		
		int pasos_rotate = ROTATE * 17960.f / 6.28;
		int pasos_zz = POS_Zz * 17960.f / 6.28; 
		int pasos_xx = POS_Xx * 17960.f / 6.28;
		int pasos_yy = POS_Yy * 17960.f / 6.28;
		
		qDebug()<<"pasos"<<pasos_zz<<pasos_xx<<pasos_yy<<pasos_rotate;
		
			
			// limites de los recorridos de los motores
			if ( pasos_xx > MIN_X && pasos_xx < MAX_X && pasos_yy > MIN_Y && pasos_yy < MAX_Y && pasos_zz > MIN_Z && pasos_zz < MAX_Z && pasos_rotate > MIN_TILT &&  pasos_rotate < MAX_TILT)
			{
				qDebug()<<"stting positios"<<"front"<<POS_Zz<<"right"<<POS_Xx<<"left"<<POS_Yy;
				RoboCompJointMotor::MotorGoalPosition p_goal;
				RoboCompJointMotor::MotorGoalPositionList list;
				
				p_goal.name = "neckRight";
				p_goal.position = POS_Xx;
				list.push_back(p_goal);
				
				p_goal.name = "neckLeft";
				p_goal.position = POS_Yy;
				list.push_back(p_goal);
				
				p_goal.name = "neckFront";
				p_goal.position = POS_Zz;
				list.push_back(p_goal);

				
				p_goal.name = "neck";
				p_goal.position = ROTATE;
				list.push_back(p_goal);
				
				
				p_goal.name = "rightPan";
				p_goal.position = rotations_eyeA;
				list.push_back(p_goal);
				
							
				p_goal.name = "leftPan";
				p_goal.position = rotations_eyeB;
				list.push_back(p_goal);
				
				
				p_goal.name = "tilt";
				p_goal.position = eyestilt;
				list.push_back(p_goal);
			
				mutex->lock();
				try
				{
					jointmotor1_proxy->setSyncPosition( list );
				}
				catch( Ice::Exception & ex)
				{
					std::cout<< ex.what()<< std::endl;
				}
				mutex->unlock();
			}
		
}
void SpecificWorker::setNMotorsPosition(const RoboCompJointMotor::MotorGoalPositionList& listGoals){
	RoboCompJointMotor::MotorGoalPositionList list;
	RoboCompJointMotor::MotorGoalPosition g;
	
	for(uint i=0; i<listGoals.size(); i++)
	{
		if(listGoals[i].name=="leftPan")
		{
			g.maxSpeed = 0;
			g.name = leftPanMotorName;
			g.position = listGoals[i].position;
			list.push_back(g);
		}
		else
		{
			if(listGoals[i].name=="rightPan")
			{
				g.maxSpeed = 0;
				g.name = rightPanMotorName;
				g.position = listGoals[i].position;
				list.push_back(g);
			}
			else
			{
				if(listGoals[i].name=="tilt")
				{
					g.maxSpeed = 0;
					g.name = tiltMotorName;
					g.position = listGoals[i].position;
					list.push_back(g);
				}
				else
				{
					if(listGoals[i].name=="neck")
					{
						g.maxSpeed = 0;
						g.name = neckMotorName;
						g.position = listGoals[i].position;
						list.push_back(g);
					}
				}
			}
		}
	}
	if(list.size()>0)
	{
		QMutexLocker lock(mutex);
		try
		{
			jointmotor1_proxy->setSyncPosition( list );
		}
		catch( RoboCompJointMotor::UnknownMotorException & ex)
		{
			throw ex;
		}
		catch( RoboCompJointMotor::HardwareFailedException & ex)
		{
			throw ex;
		}
	}
}