/**
 * @brief Check if some point ahead on the road is closer (L2) than along the road, to take a shortcut
 * 
 * @param innermodel ...
 * @param road ...
 * @param laserData ...
 * @return bool
 */
bool ElasticBand::shortCut(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
	//Compute distances from robot to all points ahead. If any of them is laser-visible and  significantly shorter than de distance along de road, try it!
	WayPoints::iterator robot = road.getIterToClosestPointToRobot();
	WayPoints::iterator best = road.begin();
	for (WayPoints::iterator it = robot + 1; it != road.end(); ++it)
	{
		//qDebug() << __FUNCTION__ << it->isVisible << (it->pos - robot->pos).norm2() << road.computeDistanceBetweenPointsAlongRoad(robot, it);
		if ( it->isVisible )
		{
			if (road.computeDistanceBetweenPointsAlongRoad(robot, it) - (it->pos - robot->pos).norm2() >  300)  //Half robot SACARRRR
			{
				qDebug() << __FUNCTION__ << "Candidato";
				//Check if the robot passes through the straight line
				if (checkCollisionAlongRoad(innermodel, laserData, road, robot, it, ROBOT_RADIUS))
				{
					//Is so remove all intermadiate points between robot and new subtarget
					qDebug() << __FUNCTION__ << "Confirmado";
					best = it;
				}
			}
		}
		else
			break;
	}
	if (best != road.begin() and (robot + 1) != road.end())
		road.erase(robot + 1, best);
	return false;
}
bool ElasticBand::update(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData,
                         const CurrentTarget &currentTarget, uint iter)
{
	//qDebug() << __FILE__ << __FUNCTION__ << "road size"<<  road.size();
	if (road.isFinished() == true)
		return false;

	/////////////////////////////////////////////
	//Tags all points in the road ar visible or blocked, depending on laser visibility. Only visible points are processed in this iteration
	/////////////////////////////////////////////
	//checkVisiblePoints(innermodel, road, laserData);

	/////////////////////////////////////////////
	//Check if there is a sudden shortcut to take
	/////////////////////////////////////////////
	//shortCut(innermodel, road, laserData);

	/////////////////////////////////////////////
	//Add points to achieve an homogenoeus chain
	/////////////////////////////////////////////
	addPoints(road, currentTarget);

	/////////////////////////////////////////////
	//Remove point too close to each other
	/////////////////////////////////////////////
	cleanPoints(road);

	/////////////////////////////////////////////
	//Compute the scalar magnitudes
	/////////////////////////////////////////////
	computeForces(innermodel, road, laserData);

	/////////////////////////////////////////////
	//Delete half the tail behind, if greater than 6, to release resources
	/////////////////////////////////////////////
	if (road.getIndexOfClosestPointToRobot() > 6)
	{
		for (auto it = road.begin(); it != road.begin() + (road.getIndexOfCurrentPoint() / 2); ++it)
			road.backList.append(it->pos);
		road.erase(road.begin(), road.begin() + (road.getIndexOfCurrentPoint() / 2));
	}
	return true;
}