inline constexpr
bool
operator>=(const position<TTag, TDistance1>& x,
           const position<TTag, TDistance2>& y)
{
    return x.distance_from_origin() >= y.distance_from_origin();
}

void init(int       argc,
          char      **argv,
          uint      displayMode,
          position  initPos,
          size      initSize,
          string    windowTitle
          )
{
    glutInit(&argc, argv);
    //Simple buffer
    glutInitDisplayMode(displayMode);
    glutInitWindowPosition(initPos.getX(),initPos.getY());
    glutInitWindowSize(initSize.getWidth(),initSize.getHeight());
    glutCreateWindow(windowTitle.c_str());
    initRendering();

    glutDisplayFunc(draw);
    glutIdleFunc(animate);

    glutKeyboardFunc(keyboardListener);
    glutSpecialFunc(specialKeyListener);


    glutMouseFunc(mouseListener);

    //Call to the drawing function


}

Shell::Shell(position startPos) // Constructor
{
	pos.set(startPos.getX(), startPos.getY(), startPos.getTh());
	seeTank = false;
	seeAi = false;
	float x,y;
	x = pos.getX();
	y = pos.getY();
	updateBb();
}

	position treeSearch(object e,position P) //return a position after searching an element
	{
		if (isExternal(P))
			return P;

		if (P.element()==e)
			return P;
		else if (e<P.element())
			return treeSearch(e,leftChild(P));
		else 
			return treeSearch(e,rightChild(P));
	}

inline constexpr
position<TTag, TToDistance>
position_cast(const position<TTag, TFromDistance>& pos)
{
    return position<TTag, TToDistance>(distance_cast<TToDistance>(
                            pos.distance_from_origin()));
}

bool
RegionFilteredSensor::filterContact(const position & boundaryPos0, const position & boundaryPos1, const position & ps){
	double p0x = boundaryPos0.x();
	double p0y = boundaryPos0.y();
	double p0z = boundaryPos0.z();
	double p1x = boundaryPos1.x();
	double p1y = boundaryPos1.y();
	double p1z = boundaryPos1.z();

	if((p0x <= ps[0] && p0y <= ps[1] && p0z <= ps[2]) &&
		(p1x >= ps.x() && p1y >= ps.y() && p1z >= ps.z())){
		return true;
	}
	return false;
}

/// Returns true if the specified position is inside (inclusive)
/// the region.
///
/// @param[in] p Point to test.
/// @retval true Point is in the region or on regions boundary.
/// @retval false Point outside the region.
///
bool region::inside(const position & p) const
{
	// testing latitude is easy, there is no date line, no wrap-arround
	if (p.lat() > p0_.lat())
		return false;
	if (p.lat() < p1_.lat())
		return false;

	// testint longitude

	// shifted longitudes, now between 0..360 (date line, eastwards)
	const double plon_s = 180.0 + p.lon();
	const double p0lon_s = 180.0 + p0_.lon();
	const double p1lon_s = 180.0 + p1_.lon();

	// p is west of p0, but not west enough to reach p1
	if ((plon_s < p0lon_s) && (plon_s > p1lon_s))
		return false;

	return true;
}

bool operator<(const position & a, const position & b)
{
    if(a.x() < b.x() || a.y() < b.y() || a.z() < b.z())
        return true;
    return false;

}

/// Initializes the region by the specified corners
///
/// @code
/// a0
/// +--------------+
/// |              |
/// |              |
/// |              |
/// +--------------+ a1
/// @endcode
///
/// @note There is no sorting of coordinates, the positions a0/a1 must
///       already be top/left,bottom/right ordering. There is no automatic
///       way to sort them because of the international date line.
///
/// @param[in] a0 Position top/left
/// @param[in] a1 Position bottom/right
/// @exception std::invalid_argument Positions are not correct. The latitude
///   of the second point <tt>p1</tt> is northerly than <tt>p0</tt>. Or positions
///   are party or fully the same.
///
region::region(const position & a0, const position & a1)
	: p0_(a0)
	, p1_(a1)
{
	if ((a0.lat() == a1.lat()) || (a0.lon() == a1.lon()))
		throw std::invalid_argument{"specified region lacks a dimension"};
	if (a0.lat() < a1.lat())
		throw std::invalid_argument{"specified region is upside down"};
}

void add(){
  node *prevlast = 0;
  while (last.pos == len && last.to != root)
    last = go(last.to->p->lnk,last.to->L,last.pos);
  while (!last.can(s[len]-'a')) {
    node* lastnode = split(last.to, last.pos);
    addleaf(lastnode,len);
    if (prevlast) prevlast->lnk = lastnode;
    prevlast = lastnode;
    if (lastnode == root) {
      last = position(root,0);
      break;
    }
    assert(lastnode->p->lnk);
    last = go(lastnode->p->lnk,lastnode->L,last.pos);
  }
  if (prevlast && !prevlast->lnk){
    assert(last.to && last.pos == last.to->R);
    prevlast->lnk = last.to;
  }
  assert(last.can(s[len]-'a'));
  last.next(s[len]-'a');
  len++;
}

std::vector<int> person::choose_move (position &pos, bool p)
{
	in_out[options[0]]->draw(pos);
	in_out[options[0]]->message("You are ");
	if (p)
	{	
		in_out[options[0]]->message("red");
	}
	else
	{
		in_out[options[0]]->message("black");
	}
	in_out[options[0]]->message(".\n\nWhich piece do you want to move?\n");
	std::vector<int> o = in_out[options[0]]->choose_point();
	if (pos.board[o[0]][o[1]] * (2 *!p - 1) <= 0)
	{
		in_out[options[0]]->message("Please choose one of your pieces.\n\n");
		return choose_move(pos, p);
	}
	in_out[options[0]]->message("\nWhere do you want to move it?\n");
	std::vector<int> n = in_out[options[0]]->choose_point();
	std::vector<int> m;
	bool is_valid = false;
	std::vector<std::vector<int> > z;
	pos.valid_moves(z, p);
	for (unsigned int i = 0; i < z.size(); i ++)
	{
		if ((o[0] == z[i][0]) && (o[1] == z[i][1]) && (n[0] == z[i][2]) && (n[1] == z[i][3]))
		{
			is_valid = true;
		}
	}
	if (is_valid)
	{
		m.push_back(o[0]);
		m.push_back(o[1]);
		m.push_back(n[0]);
		m.push_back(n[1]);
		return m;
	}
	else
	{
		in_out[options[0]]->message("\nPlease choose a valid move.\n\n");
		return choose_move(pos, p);
	}
}

void navCallback(const ardrone_autonomy::Navdata &msg)
{
	static bool start=true;
	static float last_time = 0;
	if(start){
		start = false;
		pos.pos_f(0) = -1.95;
		pos.pos_f(1) = 0;
		pos.pos_b(0) = -1.95;
		pos.pos_b(1) = 0;
		last_time = msg.tm;
	}
	geometry_msgs::PoseStamped rawpos_b;
	geometry_msgs::PoseStamped rawpos_f;
	Vector3f raw_v ;

	
	raw_v(0) = msg.vx;
	raw_v(1) = msg.vy;
	raw_v(2) = msg.vz;

	float dt = (msg.tm - last_time)/1000000.0;
	last_time = msg.tm;
	pos.pos_b += raw_v * dt / 1000.0;
	pos.get_R_field_body(pos.yaw/57.3);
	pos.pos_f = pos.R_field_body * pos.pos_b;

	rawpos_b.pose.position.x = pos.pos_b(0);
	rawpos_b.pose.position.y = pos.pos_b(1);
	rawpos_b.pose.position.z = pos.pos_b(2);
	rawpos_f.pose.position.x = pos.pos_f(0);
	rawpos_f.pose.position.y = pos.pos_f(1);
	rawpos_f.pose.position.z = pos.pos_f(2);

	rawpos_b_pub.publish(rawpos_b);
	rawpos_f_pub.publish(rawpos_f);

	// 0: Unknown
	// 1: Inited
	// 2: Landed
	// 3,7: Flying
	// 4: Hovering
	// 5: Test (?)
	// 6: Taking off
	// 8: Landing
	// 9: Looping (?)
	flight.drone_state = msg.state;
}

location location::span(position b, unsigned n) {
    return location(b, position(b.row(), b.col() + n));
}

bool position::operator<(const position &other) const {
    if (row() < other.row()) return true;
    if (row() > other.row()) return false;
    return col() < other.col();
}

	nodeptr toPointer(position P) //return the pointer saved in position P
	{
		return P.pointer();
	}

bool operator<(const position& a, const position& b) {
    assert(&a.source() == &b.source());
    return a.index() < b.index();
}

//circle position
void position_reset_info_Callback(const image_process::drone_info msg)
{
    pos.pos_img(0) = msg.pose.x;
    pos.pos_img(1) = msg.pose.y;
    position_reset_stamp = ros::Time::now();
}

int main(int argc, char **argv)
{
	
	ros::init(argc, argv, "pingpong");
	ros::NodeHandle n;
	ros::Publisher cmd_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
	ros::Publisher takeoff_pub = n.advertise<std_msgs::Empty>("/ardrone/takeoff", 1);
	ros::Publisher land_pub = n.advertise<std_msgs::Empty>("/ardrone/land", 1);
	ros::Publisher stop_pub = n.advertise<std_msgs::Empty>("/ardrone/reset", 1);
	rawpos_b_pub = n.advertise<geometry_msgs::PoseStamped>("/ardrone/rawpos_b", 1);
	rawpos_f_pub = n.advertise<geometry_msgs::PoseStamped>("/ardrone/rawpos_f", 1);
	ros::Publisher ctrl_pub = n.advertise<flight_strategy::ctrl>("ctrl", 1);
	ros::Subscriber ctrlBack_sub = n.subscribe("ctrlBack", 1, ctrlBack_Callback);
	ros::Subscriber robot_info_sub = n.subscribe("/ardrone/robot_info", 1, robot_info_Callback);
	ros::Subscriber position_reset_info_sub = n.subscribe("/ardrone/position_reset_info", 1, position_reset_info_Callback);
	ros::Subscriber nav_sub = n.subscribe("/ardrone/navdata", 1, navCallback);
	ros::Subscriber altitude_sub = n.subscribe("/ardrone/navdata_altitude", 1, altitudeCallback);
	ros::Subscriber yaw_sub = n.subscribe("/ardrone/yaw", 1, yawCallback);
	//ros::Subscriber catch_pos_sub = n.subscribe("", 1, catch_pos_Callback);
	ros::Subscriber robot_pos_sub = n.subscribe("/ardrone/robot_position", 1, robot_pos_Callback);


	ros::Rate loop_rate(LOOP_RATE);
	std_msgs::Empty order;
	//geometry_msgs::Twist cmd;

	catch_position(0) = -1000;
	catch_position(1) = -1000;
	robot.pos_f[0] = -1.95;
	robot.pos_f[1] = 0;

	while(ros::ok())
	{
		switch(flight.state){
			case STATE_IDLE:{
				if(flight.last_state != flight.state){
					ROS_INFO("State to IDLE\n");
				}
				flight.last_state = flight.state;
				flight.state = STATE_TAKEOFF;
				break;
			}
			case STATE_TAKEOFF:{
				if(flight.last_state != flight.state){
					ROS_INFO("State TAKEOFF\n");
					takeoff_pub.publish(order);
				}
				flight.last_state = flight.state;
				if(flight.drone_state != 6){
					//fly to center
					flight.state = STATE_LOCATING;
				}
				break;
			}
			case STATE_LOCATING:{
				static unsigned int loop_times = 0;
				flight_strategy::ctrl ctrl_msg;
				
				if(flight.last_state != flight.state){
					ROS_INFO("State LOCATING\n");
					flight.last_state = flight.state;
					ctrl_msg.enable = 1;
					ctrl_msg.mode = NORMAL_CTL;
					ctrl_msg.pos_halt[0] = 1;
					ctrl_msg.pos_halt[1] = 1;
					ctrl_msg.pos_halt[2] = 1;
					ctrl_pub.publish(ctrl_msg);
					ctrl.flag_arrived = false;

				}else
				{
					if(!arrived_inaccurate){
						ctrl_msg.enable = 1;
						ctrl_msg.mode = NORMAL_CTL;
						ctrl_msg.pos_sp[0] =  -1.95;
						ctrl_msg.pos_sp[1] =  0;
						ROS_INFO("POSITION:%f   %f",pos.pos_f(0),pos.pos_f(1));
						ROS_INFO("State LOCATING:%f   %f",ctrl_msg.pos_sp[0],ctrl_msg.pos_sp[1]);
						ctrl_msg.pos_halt[0] = 0;
						ctrl_msg.pos_halt[1] = 0;
						ctrl_msg.pos_halt[2] = 1;
						ctrl_pub.publish(ctrl_msg);

						delay_counter ++;
						if(delay_counter > 10)
						{
							delay_counter = 0;
							if(ctrl.flag_arrived){
							arrived_inaccurate = true;
							ROS_INFO("POSITION RESET POSITION");
							} 	
						}

						
					}
				
					if(arrived_inaccurate)
					{
						ros::Duration dur;
						dur = ros::Time::now() - position_reset_stamp;
//.........这里部分代码省略.........

 constexpr
 position(const position<tag, TDistance2>& pos)
     : m_distance(pos.distance_from_origin())
 {
 }

//altitude of sonar
void altitudeCallback(const ardrone_autonomy::navdata_altitude &msg)
{
	pos.pos_f(2) = msg.altitude_vision/1000.0;
	//ROS_INFO("altitude:%f",pos.pos_f(2));
}