/**
 * @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 Check if any of the waypoints has nan coordinates
 * 
 * @param road ...
 * @return bool
 */
bool ElasticBand::checkIfNAN(const WayPoints &road)
{
	for (auto it = road.begin(); it != road.end(); ++it)
		if (std::isnan(it->pos.x()) or std::isnan(it->pos.y()) or std::isnan(it->pos.z()))
		{
			road.print();
			return true;
		}
	return false;
}

/**
 * @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;
}

/**
 * @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 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;
}

/**
 * @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);
		}
	}
}

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;
}

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;
}

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;
}

bool LineFollower::update(InnerModel *innerModel, RoboCompLaser::TLaserData &laserData, RoboCompOmniRobot::OmniRobotPrx omnirobot_proxy, WayPoints &road)
{
	static QTime reloj = QTime::currentTime();   //TO be used for a more accurate control (predictive).
	/*static*/ long epoch = 100;
	static float lastVadvance = 0.f;
	const float umbral = 25.f;	//salto maximo de velocidad
	static float lastVrot = 0.f;
	const float umbralrot = 0.08f;	//salto maximo de rotación

	//Estimate the space that will be blindly covered and reduce Adv speed to remain within some boundaries
	//qDebug() << __FILE__ << __FUNCTION__ << "entering update with" << road.at(road.getIndexOfClosestPointToRobot()).pos;

	//Check robot state
	if( (road.isFinished() == true ) or (road.requiresReplanning== true) or (road.isLost == true))
	{
		 		if( road.isFinished() ) qDebug() << "road finished";
		 		if( road.requiresReplanning ) qDebug() << "requiresReplanning";
		 		if( road.isLost ) qDebug() << "robot is lost";
		stopTheRobot(omnirobot_proxy);

		return false;
	}

	///CHECK ROBOT INMINENT COLLISION
	float vside = 0;
	int j=0;
	road.setBlocked(false);
	for(auto i : laserData)
	{
		//printf("laser dist %f || baseOffsets %f \n",i.dist,baseOffsets[j]);
		if(i.dist < 10) i.dist = 30000;
		if( i.dist < baseOffsets[j] + 50 )
		{
			if(i.angle>-1.30 && i.angle<1.30){
			qDebug() << __FILE__ << __FUNCTION__<< "Robot stopped to avoid collision because distance to obstacle is less than " << baseOffsets[j] << " "<<i.dist << " " << i.angle;
			stopTheRobot(omnirobot_proxy);
			road.setBlocked(true);		//AQUI SE BLOQUEA PARA REPLANIFICAR
			qDebug()<<"DETECTADO OBSTACULO, REPLANIFICANDO";
 			break;
			}
		}
		else
		{
			if (i.dist < baseOffsets[j] + 150) 
			{
				if (i.angle > 0)
				{
					vside  = -80;
				}
				else
				{
					vside = 80;
				}
			}
		}
		j++;
	}

	/////////////////////////////////////////////////
	//////  CHECK CPU AVAILABILITY
	/////////////////////////////////////////////////
	if ( time.elapsed() > delay )   //Initial wait in secs so the robot waits for everything is setup. Maybe it could be moved upwards
	{
		float MAX_ADV_SPEED = 200.f;
		float MAX_ROT_SPEED = 0.3;
		if( (epoch-100) > 0 )				//Damp max speeds if elapsed time is too long
		{
			MAX_ADV_SPEED = 200 * exponentialFunction(epoch-100, 200, 0.2);
			MAX_ROT_SPEED = 0.3 * exponentialFunction(epoch-100, 200, 0.2);
		}
		float vadvance = 0;
		float vrot = 0;
		/////////////////////////////////////////////////
		//////   ROTATION SPEED
		////////////////////////////////////////////////

		// VRot is computed as the sum of three terms: angle with tangent to road + atan(perp. distance to road) + road curvature
		// as descirbed in Thrun's paper on DARPA challenge
		vrot = road.getAngleWithTangentAtClosestPoint() + atan( road.getRobotPerpendicularDistanceToRoad()/800.) + 0.8 * road.getRoadCurvatureAtClosestPoint() ;  //350->800.
	// Limiting filter
 		if( vrot > MAX_ROT_SPEED )
 			vrot = MAX_ROT_SPEED;
 		if( vrot < -MAX_ROT_SPEED )
 			vrot = -MAX_ROT_SPEED;

		/////////////////////////////////////////////////
		//////   ADVANCE SPEED
		////////////////////////////////////////////////

		// Factor to be used in speed control when approaching the end of the road
		float teta;
		if( road.getRobotDistanceToTarget() < 1000)
			teta = exponentialFunction(1./road.getRobotDistanceToTarget(),1./500,0.5, 0.1);
		else
			teta= 1;
		
		// Factor to be used in speed control when approaching the end of the road
		

		//VAdv is computed as a reduction of MAX_ADV_SPEED by three computed functions:
//.........这里部分代码省略.........

static int getNextWaypoint()
{
  int path_size = static_cast<int>(_current_waypoints.getSize());
  double lookahead_threshold = getLookAheadThreshold(0);

  // if waypoints are not given, do nothing.
  if (_current_waypoints.isEmpty())
  {
    return -1;
  }

  // look for the next waypoint.
  for(int i = 0; i < path_size; i++)
  {

    //if search waypoint is the last
    if (i == (path_size - 1))
    {
      ROS_INFO("search waypoint is the last");
      return i;
    }

    // if there exists an effective waypoint
    if (getPlaneDistance(_current_waypoints.getWaypointPosition(i), _current_pose.pose.position) > lookahead_threshold)
    {
      return i;
    }
  }

  //if this program reaches here , it means we lost the waypoint!
  return -1;
}

static void ChangeWaypoint(EControl detection_result)
{

  int obs = _obstacle_waypoint;

  if (obs != -1){
    std::cout << "====got obstacle waypoint====" << std::endl;
    std::cout << "=============================" << std::endl;
  }

  if (detection_result == STOP){ // STOP for obstacle
    // stop_waypoint is about _others_distance meter away from obstacles
    int stop_waypoint = obs - ((int)(_others_distance / _path_change.getInterval()));
    std::cout << "stop_waypoint: " << stop_waypoint << std::endl;
    // change waypoints to stop by the stop_waypoint
    _path_change.changeWaypoints(stop_waypoint);
    _path_change.avoidSuddenBraking();
    _path_change.setTemporalWaypoints();
    _temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints());
  } else if (detection_result == DECELERATE) { // DECELERATE for obstacles
    _path_change.setPath(_path_dk.getCurrentWaypoints());
    _path_change.setDeceleration();
    _path_change.setTemporalWaypoints();
    _temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints());
  } else {               // ACELERATE or KEEP
    _path_change.setPath(_path_dk.getCurrentWaypoints());
    _path_change.avoidSuddenAceleration();
    _path_change.avoidSuddenBraking();
    _path_change.setTemporalWaypoints();
    _temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints());
  }


    return;
}

static void doPurePursuit()
{
  //search next waypoint
  int next_waypoint = getNextWaypoint();
  displayNextWaypoint(next_waypoint);
  displaySearchRadius(getLookAheadThreshold(0));
  //ROS_INFO_STREAM("next waypoint = " << next_waypoint << "/" << path_size - 1);

  //linear interpolation and calculate angular velocity
  geometry_msgs::Point next_target;
  geometry_msgs::TwistStamped twist;
	std_msgs::Bool wf_stat;
	bool interpolate_flag = false;

  if (!g_linear_interpolate_mode)
  {
    next_target = _current_waypoints.getWaypointPosition(next_waypoint);
  }
  else
  {
    interpolate_flag = interpolateNextTarget(next_waypoint, &next_target);
  }

  if (g_linear_interpolate_mode && !interpolate_flag)
  {
    ROS_INFO_STREAM("lost target! ");
    wf_stat.data = false;
    _stat_pub.publish(wf_stat);
    twist.twist.linear.x = 0;
    twist.twist.angular.z = 0;
    twist.header.stamp = ros::Time::now();
    g_cmd_velocity_publisher.publish(twist);
    return;
  }

  //ROS_INFO("next_target : ( %lf , %lf , %lf)", next_target.x, next_target.y,next_target.z);
  displayNextTarget(next_target);
  displayTrajectoryCircle(generateTrajectoryCircle(next_target));
  twist.twist = calcTwist(calcCurvature(next_target), getCmdVelocity(0));

  wf_stat.data = true;
  _stat_pub.publish(wf_stat);
  twist.header.stamp = ros::Time::now();
  g_cmd_velocity_publisher.publish(twist);

  //ROS_INFO("linear : %lf, angular : %lf",twist.twist.linear.x,twist.twist.angular.z);

#ifdef LOG
      std::ofstream ofs("/tmp/pure_pursuit.log", std::ios::app);
      ofs << _current_waypoints.getWaypointPosition(next_waypoint).x << " "
      << _current_waypoints.getWaypointPosition(next_waypoint).y << " " << next_target.x << " " << next_target.y
      << std::endl;
#endif
}

static int FindCrossWalk()
{
  if (!vmap.set_points || _closest_waypoint < 0)
    return -1;

  double find_distance = 2.0*2.0; // meter
  double ignore_distance = 20.0*20.0; // meter
  // Find near cross walk
  for (int num = _closest_waypoint; num < _closest_waypoint+_search_distance; num++) {
    geometry_msgs::Point waypoint = _path_dk.getWaypointPosition(num);
    waypoint.z = 0.0; // ignore Z axis
    for (int i = 1; i < vmap.getSize(); i++) {
      // ignore far crosswalk
      geometry_msgs::Point crosswalk_center = vmap.getDetectionPoints(i).center;
      crosswalk_center.z = 0.0;
      if (CalcSquareOfLength(crosswalk_center, waypoint) > ignore_distance)
	continue;

      for (auto p : vmap.getDetectionPoints(i).points) {
	p.z = waypoint.z;
	if (CalcSquareOfLength(p, waypoint) < find_distance) {
	  vmap.setDetectionCrossWalkID(i);
	  return num;
	}
      }

    }
  }

  vmap.setDetectionCrossWalkID(-1);
  return -1; // no near crosswalk
}

static bool verifyFollowing()
{
  double slope = 0;
  double intercept = 0;
  getLinearEquation(_current_waypoints.getWaypointPosition(1),_current_waypoints.getWaypointPosition(2),&slope,&intercept);
  double displacement = getDistanceBetweenLineAndPoint(_current_pose.pose.position,slope,intercept);
  double relative_angle = getRelativeAngle(_current_waypoints.getWaypointPose(1),_current_pose.pose);
  //ROS_INFO("side diff : %lf , angle diff : %lf",displacement,relative_angle);
  if(displacement < g_displacement_threshold || relative_angle < g_relative_angle_threshold){
    //ROS_INFO("Following : True");
    return true;
  }
  else{
    //ROS_INFO("Following : False");
    return false;
  }
}

static double getCmdVelocity(int waypoint)
{

  if (_param_flag == MODE_DIALOG)
  {
    ROS_INFO_STREAM("dialog : " << _initial_velocity << " km/h (" << kmph2mps(_initial_velocity) << " m/s )");
    return kmph2mps(_initial_velocity);
  }

  if (_current_waypoints.isEmpty())
  {
    ROS_INFO_STREAM("waypoint : not loaded path");
    return 0;
  }

  double velocity = _current_waypoints.getWaypointVelocityMPS(waypoint);
  //ROS_INFO_STREAM("waypoint : " << mps2kmph(velocity) << " km/h ( " << velocity << "m/s )");
  return velocity;
}

void BaseWaypointCallback(const waypoint_follower::laneConstPtr &msg)
{
  ROS_INFO("subscribed base_waypoint\n");
  _path_dk.setPath(*msg);
  _path_change.setPath(*msg);
  if (_path_flag == false) {
    std::cout << "waypoint subscribed" << std::endl;
    _path_flag = true;
  }

}

void PathVset::changeWaypoints(int stop_waypoint)
{
  int i = 0;
  int close_waypoint_threshold = 4;
  int fill_in_zero = 20;
  double changed_vel;
  double interval = getInterval();

  // change waypoints to decelerate
  for (int num = stop_waypoint; num > _closest_waypoint - close_waypoint_threshold; num--){
    if (!checkWaypoint(num, "changeWaypoints"))
      continue;

    changed_vel = sqrt(2.0*_decel*(interval*i)); // sqrt(2*a*x)

    //std::cout << "changed_vel[" << num << "]: " << mps2kmph(changed_vel) << " (km/h)";
    //std::cout << "   distance: " << (_obstacle_waypoint-num)*interval << " (m)";
    //std::cout << "   current_vel: " << mps2kmph(_current_vel) << std::endl;

    waypoint_follower::waypoint initial_waypoint = _path_dk.getCurrentWaypoints().waypoints[num];
    if (changed_vel > _velocity_limit || //
	changed_vel > initial_waypoint.twist.twist.linear.x){ // avoid acceleration
      //std::cout << "too large velocity!!" << std::endl;
      current_waypoints_.waypoints[num].twist.twist.linear.x = initial_waypoint.twist.twist.linear.x;
    } else {
      current_waypoints_.waypoints[num].twist.twist.linear.x = changed_vel;
    }

    i++;
  }

  // fill in 0
  for (int j = 1; j < fill_in_zero; j++){
    if (!checkWaypoint(stop_waypoint+j, "changeWaypoints"))
      continue;
    current_waypoints_.waypoints[stop_waypoint+j].twist.twist.linear.x = 0.0;
  }


  std::cout << "---changed waypoints---" << std::endl;

  return;
}

// display the next waypoint by markers.
static void displayNextWaypoint(int i)
{

  visualization_msgs::Marker marker;
  marker.header.frame_id = MAP_FRAME;
  marker.header.stamp = ros::Time();
  marker.ns = "next_waypoint_marker";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.action = visualization_msgs::Marker::ADD;
  marker.pose.position = _current_waypoints.getWaypointPosition(i);
  marker.scale.x = 1.0;
  marker.scale.y = 1.0;
  marker.scale.z = 1.0;
  marker.color.a = 1.0;
  marker.color.r = 0.0;
  marker.color.g = 0.0;
  marker.color.b = 1.0;
  marker.frame_locked = true;
  _vis_pub.publish(marker);
}

static void DisplayDetectionRange(const int &crosswalk_id, const int &num, const EControl &kind)
{
    // set up for marker array
    visualization_msgs::MarkerArray marker_array;
    visualization_msgs::Marker crosswalk_marker;
    visualization_msgs::Marker waypoint_marker_stop;
    visualization_msgs::Marker waypoint_marker_decelerate;
    visualization_msgs::Marker stop_line;
    crosswalk_marker.header.frame_id = "/map";
    crosswalk_marker.header.stamp = ros::Time();
    crosswalk_marker.id = 0;
    crosswalk_marker.type = visualization_msgs::Marker::SPHERE_LIST;
    crosswalk_marker.action = visualization_msgs::Marker::ADD;
    waypoint_marker_stop = crosswalk_marker;
    waypoint_marker_decelerate = crosswalk_marker;
    stop_line = crosswalk_marker;
    stop_line.type = visualization_msgs::Marker::CUBE;


    // set each namespace
    crosswalk_marker.ns = "Crosswalk Detection";
    waypoint_marker_stop.ns = "Stop Detection";
    waypoint_marker_decelerate.ns = "Decelerate Detection";
    stop_line.ns = "Stop Line";


    // set scale and color
    double scale = 2*_detection_range;
    waypoint_marker_stop.scale.x = scale;
    waypoint_marker_stop.scale.y = scale;
    waypoint_marker_stop.scale.z = scale;
    waypoint_marker_stop.color.a = 0.2;
    waypoint_marker_stop.color.r = 0.0;
    waypoint_marker_stop.color.g = 1.0;
    waypoint_marker_stop.color.b = 0.0;
    waypoint_marker_stop.frame_locked = true;

    scale = 2*(_detection_range + _deceleration_range);
    waypoint_marker_decelerate.scale.x = scale;
    waypoint_marker_decelerate.scale.y = scale;
    waypoint_marker_decelerate.scale.z = scale;
    waypoint_marker_decelerate.color.a = 0.15;
    waypoint_marker_decelerate.color.r = 1.0;
    waypoint_marker_decelerate.color.g = 1.0;
    waypoint_marker_decelerate.color.b = 0.0;
    waypoint_marker_decelerate.frame_locked = true;

    if (_obstacle_waypoint > -1) {
      stop_line.pose.position = _path_dk.getWaypointPosition(_obstacle_waypoint);
      stop_line.pose.orientation = _path_dk.getWaypointOrientation(_obstacle_waypoint);
    }
    stop_line.pose.position.z += 1.0;
    stop_line.scale.x = 0.1;
    stop_line.scale.y = 15.0;
    stop_line.scale.z = 2.0;
    stop_line.color.a = 0.3;
    stop_line.color.r = 1.0;
    stop_line.color.g = 0.0;
    stop_line.color.b = 0.0;
    stop_line.lifetime = ros::Duration(0.1);
    stop_line.frame_locked = true;


    if (crosswalk_id > 0)
      scale = vmap.getDetectionPoints(crosswalk_id).width;
    crosswalk_marker.scale.x = scale;
    crosswalk_marker.scale.y = scale;
    crosswalk_marker.scale.z = scale;
    crosswalk_marker.color.a = 0.5;
    crosswalk_marker.color.r = 0.0;
    crosswalk_marker.color.g = 1.0;
    crosswalk_marker.color.b = 0.0;
    crosswalk_marker.frame_locked = true;


    // set marker points coordinate
    for (int i = 0; i < _search_distance; i++) {
      if (num < 0 || i+num > _path_dk.getSize() - 1)
	break;

      geometry_msgs::Point point;
      point = _path_dk.getWaypointPosition(num+i);

      waypoint_marker_stop.points.push_back(point);

      if (i > _deceleration_search_distance)
	continue;
      waypoint_marker_decelerate.points.push_back(point);
    }

    if (crosswalk_id > 0) {
      for (const auto &p : vmap.getDetectionPoints(crosswalk_id).points)
	crosswalk_marker.points.push_back(p);
    }


    // publish marker
    marker_array.markers.push_back(crosswalk_marker);
    marker_array.markers.push_back(waypoint_marker_stop);
    marker_array.markers.push_back(waypoint_marker_decelerate);
//.........这里部分代码省略.........