/**
 * @brief A point of the road is visible if it is between the robot and the laser beam running through it, and if the previous point was visible
 * All points in the road are updated
 * @param road ...
 * @param laserData ...
 * @return bool
 */
bool ElasticBand::checkVisiblePoints(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
	//Simplify laser polyline using Ramer-Douglas-Peucker algorithm
	std::vector<Point> points, res;
	QVec wd;
	for (auto &ld : laserData)
	{
		//wd = innermodel->laserTo("world", "laser", ld.dist, ld.angle); //OPTIMIZE THIS FOR ALL CLASS METHODS
		wd = innermodel->getNode<InnerModelLaser>("laser")->laserTo("world", ld.dist, ld.angle);
		points.push_back(Point(wd.x(), wd.z()));
	}
	res = simPath.simplifyWithRDP(points, 70); 
	//qDebug() << __FUNCTION__ << "laser polygon after simp" << res.size();

	// Create a QPolygon so we can check if robot outline falls inside
	QPolygonF polygon;
	for (auto &p: res)
		polygon << QPointF(p.x, p.y);

	// Move the robot along the road
	int robot = road.getIndexOfNextPoint();
	QVec memo = innermodel->transform6D("world", "robot");
	for(int it = robot; it<road.size(); ++it)
	{
		road[it].isVisible = true;
		innermodel->updateTransformValues("robot", road[it].pos.x(), road[it].pos.y(), road[it].pos.z(), 0, road[it].rot.y(), 0);
		//get Robot transformation matrix
		QMat m = innermodel->getTransformationMatrix("world", "robot");
		// Transform all points at one to world RS
		//m.print("m");
		//pointsMat.print("pointsMat");
		QMat newPoints = m * pointsMat;

		//Check if they are inside the laser polygon
		for (int i = 0; i < newPoints.nCols(); i++)
		{
// 			qDebug() << __FUNCTION__ << "----------------------------------";
// 			qDebug() << __FUNCTION__ << QPointF(newPoints(0, i), newPoints(2, i));
// 			qDebug() << __FUNCTION__ << polygon;
			if (polygon.containsPoint(QPointF(newPoints(0, i), newPoints(2, i)),Qt::OddEvenFill) == false)
			{
				road[it].isVisible = false;
				//qFatal("fary");
				break;
			}
		}
//		if( road[it].isVisible == false)
//		{
//			for (int k = it; k < road.size(); ++k)
//				road[k].isVisible = false;
//			break;
//		}
	}

	// Set the robot back to its original state
	innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0);

	//road.print();
	return true;
}
/**
 * @brief Adds points to the band if two existing ones are too far apart (ROBOT_RADIUS)
 * 
 * @param road ...
 * @return void
 */
void ElasticBand::addPoints(WayPoints &road, const CurrentTarget &currentTarget)
{
	int offset = 1;

	for (int i = 0; i < road.size() - offset; i++)
	{
		if (i > 0 and road[i].isVisible == false)
			break;

		WayPoint &w = road[i];
		WayPoint &wNext = road[i + 1];
		float dist = (w.pos - wNext.pos).norm2();
		if (dist > ROAD_STEP_SEPARATION)  //SHOULD GET FROM IM
		{
			float l = 0.9 * ROAD_STEP_SEPARATION / dist;   //Crucial que el punto se ponga mas cerca que la condición de entrada
			WayPoint wNew((w.pos * (1 - l)) + (wNext.pos * l));
			road.insert(i + 1, wNew);
		}
	}

	//ELIMINATED AS REQUESTED BY MANSO
	//Move point before last to orient the robot. This works but only if the robots approaches from the lower quadrants
	//The angle formed by this point and the last one has to be the same es specified in the target
	//We solve this equations for (x,z)
	// (x' -x)/(z'-z) = tg(a) = t
	// sqr(x'-x) + sqr(z'-z) = sqr(r)
	// z = z' - (r/(sqrt(t*t -1)))
	// x = x' - r(sqrt(1-(1/t*t+1)))
	// 	if( (currentTarget.hasRotation() == true) and (road.last().hasRotation == false) )
	// 	{
	// 		qDebug() << __FUNCTION__ << "computing rotation" << road.last().pos;
	// 		float radius = 500;
	// 		float ta = tan(currentTarget.getRotation().y());
	// 		float xx = road.last().pos.x() - radius*sqrt(1.f - (1.f/(ta*ta+1)));
	// 		float zz = road.last().pos.z() - (radius/sqrt(ta*ta+1));
	// 		WayPoint wNew( QVec::vec3(xx,road.last().pos.y(),zz) );
	// 		road.insert(road.end()-1,wNew);
	// 		road.last().hasRotation = true;
	// 		qDebug() << __FUNCTION__ << "after rotation" << wNew.pos << currentTarget.getRotation().y() << ta;
	// 	
	// 	}
	//else
	//qDebug() << road.last().hasRotation << road.last().pos << (road.end()-2)->pos << currentTarget.getRotation().y();

}
/**
 * @brief Removes points from the band if two of them are too close, ROBOT_RADIUS/3.
 * 
 * @param road ...
 * @return void
 */
void ElasticBand::cleanPoints(WayPoints &road)
{
	int i;
	int offset = 2;
	//if( road.last().hasRotation ) offset = 3; else offset = 2;

	for (i = 1; i < road.size() -
	                offset; i++) // exlude 1 to avoid deleting the nextPoint and the last two to avoid deleting the target rotation
	{
		if (road[i].isVisible == false)
			break;
		WayPoint &w = road[i];
		WayPoint &wNext = road[i + 1];

		float dist = (w.pos - wNext.pos).norm2();
		if (dist < ROAD_STEP_SEPARATION / 3.)
		{
			road.removeAt(i + 1);
		}
	}
}
float ElasticBand::computeForces(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
	if (road.size() < 3)
		return 0;

	// To avoid moving the rotation element attached to the last
	int lastP;
	if (road.last().hasRotation)
		lastP = road.size() - 2;
	else
		lastP = road.size() - 1;

	// Go through all points in the road
	float totalChange = 0.f;
	for (int i = 1; i < lastP; i++)
	{
		if (road[i].isVisible == false)
			break;

		WayPoint &w0 = road[i - 1];
		WayPoint &w1 = road[i];
		WayPoint &w2 = road[i + 1];

		// Atraction force caused by the trajectory stiffnes, trying to straighten itself. It is computed as a measure of local curvature
		QVec atractionForce(3);
		float n = (w0.pos - w1.pos).norm2() / ((w0.pos - w1.pos).norm2() + w1.initialDistanceToNext);
		atractionForce = (w2.pos - w0.pos) * n - (w1.pos - w0.pos);

		//Compute derivative of force field and store values in w1.bMinuxX .... and w1.minDist. Also variations wrt former epochs
		computeDistanceField(innermodel, w1, laserData, FORCE_DISTANCE_LIMIT);

		QVec repulsionForce = QVec::zeros(3);
		QVec jacobian(3);

		// space interval to compute the derivative. Related to to robot's size
		float h = DELTA_H;
		if ((w1.minDistHasChanged == true) /*and (w1.minDist < 250)*/ )
		{
			jacobian = QVec::vec3(w1.bMinusX - w1.bPlusX,
			                      0,
			                      w1.bMinusY - w1.bPlusY) * (T) (1.f / (2.f * h));

			// repulsion force is computed in the direction of maximun laser-point distance variation and scaled so it is 0 is beyond FORCE_DISTANCE_LIMIT and FORCE_DISTANCE_LIMIT if w1.minDist.
			repulsionForce = jacobian * (FORCE_DISTANCE_LIMIT - w1.minDist);

		}

		float alpha = -0.5; //Negative values between -0.1 and -1. The bigger in magnitude, the stiffer the road becomes
		float beta = 0.55;  //Posibite values between  0.1 and 1	 The bigger in magnitude, more separation from obstacles

		QVec change = (atractionForce * alpha) + (repulsionForce * beta);
		if (std::isnan(change.x()) or std::isnan(change.y()) or std::isnan(change.z()))
		{
			road.print();
			qDebug() << atractionForce << repulsionForce;
			qFatal("change");
		}
		//Now we remove the tangencial component of the force to avoid recirculation of band points
		//QVec pp = road.getTangentToCurrentPoint().getPerpendicularVector();
		//QVec nChange = pp * (pp * change);

		w1.pos = w1.pos - change;
		totalChange = totalChange + change.norm2();
	}
	return totalChange;
}