/**
 * @brief Stops the robot
 *
 * @return bool
 */
bool SpecificWorker::stopCommand(CurrentTarget &target, WayPoints &myRoad, TrajectoryState &state)
{
	controller->stopTheRobot(omnirobot_proxy);
	myRoad.setFinished(true);
	myRoad.reset();
	myRoad.endRoad();
#ifdef USE_QTGUI
	//myRoad.clearDraw(viewer->innerViewer);
	//drawGreenBoxOnTarget(target.getTranslation());
#endif
	target.reset();
	state.setElapsedTime(taskReloj.elapsed());
	state.setState("IDLE");
	qDebug() << __FUNCTION__ << "Robot at pose:" << innerModel->transform6D("world", "robot");
	return true;
}
bool
SpecificWorker::gotoCommand(InnerModel *innerModel, CurrentTarget &target, TrajectoryState &state, WayPoints &myRoad,
                            RoboCompLaser::TLaserData &lData)
{
	QTime reloj = QTime::currentTime();

	/////////////////////////////////////////////////////
	// check for ending conditions
	//////////////////////////////////////////////////////
	if (myRoad.isFinished() == true)
	{
		controller->stopTheRobot(omnirobot_proxy);
		qDebug() << __FUNCTION__ << "Changing to SETHEADING command";
		target.setState(CurrentTarget::State::SETHEADING);
		return true;
	}
 	if (myRoad.isBlocked() == true)		//Road BLOCKED, go to BLOCKED state and wait it the obstacle moves
 	{
		controller->stopTheRobot(omnirobot_proxy);
 		//currentTargetBack.setTranslation(innerModel->transform("world", QVec::vec3(0, 0, -250), "robot"));
 		target.setState(CurrentTarget::State::BLOCKED);
		return false;
 	}

	// Get here when robot is stuck
// 	if(myRoad.requiresReplanning == true)
// 	{
// 	 		//qDebug() << __FUNCTION__ << "STUCK, PLANNING REQUIRED";
// 	 		//computePlan(innerModel);
// 	}
	
	//////////////////////////////////////////
	// Check if there is a plan for the target
	//////////////////////////////////////////
	bool coolPlan = true;
	if (target.isWithoutPlan() == true)
	{
		state.setState("PLANNING");
		QVec localT = target.getTranslation();
		coolPlan = plannerPRM.computePath(localT, innerModel);
		if (coolPlan == false)
		{
			qDebug() << __FUNCTION__ << "Path NOT found. Resetting to IDLE state";
			target.setState(CurrentTarget::State::STOP);
			return false;
		}
		target.setTranslation(localT);
		//qDebug() << __FUNCTION__ << "Plan obtained of " << planner->getPath().size() << " points";
		// take inner to current values
		updateInnerModel(innerModel, state);
		target.setWithoutPlan(false);

		//Init road   REMOVE TRASH FROM HERE
		myRoad.reset();
		myRoad.readRoadFromList(plannerPRM.getPath());
		myRoad.requiresReplanning = false;
		myRoad.computeDistancesToNext();
		myRoad.startRoad();
		state.setPlanningTime(reloj.elapsed());
		state.setState("EXECUTING");
	}

	///////////////////////////////////
	// Update the band
	/////////////////////////////////
	elasticband.update(innerModel, myRoad, laserData, target);

	///////////////////////////////////
	// compute all measures relating the robot to the road
	/////////////////////////////////
	myRoad.update();

	//myRoad.printRobotState(innerModel, target);

	/////////////////////////////////////////////////////
	//move the robot according to the current force field
	//////////////////////////////////////////////////////
	controller->update(innerModel, lData, omnirobot_proxy, myRoad);

	
	#ifdef USE_QTGUI
		waypointsdraw.draw(myRoad, viewer,  target);
	#endif
	
	state.setEstimatedTime(myRoad.getETA());
	return true;
}