import rospy
from nav_msgs.msg import Odometry

from sensor_msgs.msg import LaserScan
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Twist
from ar_track_alvar_msgs.msg import AlvarMarkers
from math import copysign

class TrackerBot:
	def __init__(self):
		#state variables
		self.is_safe = True
		self.searching = True
		self.tracking = False
		#subscribers

		rospy.init_node("ar_follower")
						
		# Set the shutdown function (stop the robot)
		rospy.on_shutdown(self.shutdown)
		
		# How often should we update the robot's motion?
		self.rate = rospy.get_param("~rate", 10)
		r = rospy.Rate(self.rate) 
		
		# The maximum rotation speed in radians per second
		self.max_angular_speed = rospy.get_param("~max_angular_speed", 2.0)
		
		# The minimum rotation speed in radians per second
		self.min_angular_speed = rospy.get_param("~min_angular_speed", 0.5)
		
		# The maximum distance a target can be from the robot for us to track
		self.max_x = rospy.get_param("~max_x", 20.0)
		
		# The goal distance (in meters) to keep between the robot and the marker
		self.goal_x = rospy.get_param("~goal_x", 0.6)
		
		# How far away from the goal distance (in meters) before the robot reacts
		self.x_threshold = rospy.get_param("~x_threshold", 0.02)
		
		# How far away from being centered (y displacement) on the AR marker
		# before the robot reacts (units are meters)
		self.y_threshold = rospy.get_param("~y_threshold", 0.02)
		
		# How much do we weight the goal distance (x) when making a movement
		self.x_scale = rospy.get_param("~x_scale", 0.5)

		# How much do we weight y-displacement when making a movement		
		self.y_scale = rospy.get_param("~y_scale", 1.0)
		
		# The max linear speed in meters per second
		self.max_linear_speed = rospy.get_param("~max_linear_speed", 0.3)
		
		# The minimum linear speed in meters per second
		self.min_linear_speed = rospy.get_param("~min_linear_speed", 0.1)

		# Publisher to control the robot's movement
		self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
		
		# Intialize the movement command
		self.move_cmd = Twist()

		
		
		# Set flag to indicate when the AR marker is visible
		self.target_visible = False
		
		# Wait for the ar_pose_marker topic to become available
		rospy.loginfo("Waiting for ar_pose_marker topic...")
		rospy.wait_for_message('ar_pose_marker', AlvarMarkers)

		# rospy.wait_for_message('/scan', LaserScan)
		
		# Subscribe to the ar_pose_marker topic to get the image width and height
		rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.set_cmd_vel)
		self.lidar_sub = rospy.Subscriber("/scan", LaserScan, self.lidar_callback)
		rospy.loginfo("Marker messages detected. Starting follower...")
		


		# Begin the cmd_vel publishing loop
		while not rospy.is_shutdown():
			if self.is_safe:
				# Send the Twist command to the robot
				# rospy.loginfo("Publishing move command")
				
				self.cmd_vel_pub.publish(self.move_cmd)
			else:
				r.sleep()
				# Sleep for 1/self.rate seconds
			

	def lidar_callback(self, msg):
		distances = msg.ranges
		increment = msg.angle_increment
		safe_distance = 0.10

		count = 0
		for i in range(0,len(distances)/8):
			if distances[i] < safe_distance:
				if self.is_safe:
					self.is_safe = False
					rospy.loginfo("too close to an object robot is not safe")
				count+=1

		#checks for closest poin in range -pi/4 - 0
		for i in range(len(distances) - len(distances)/8, len(distances)):
			if distances[i] < safe_distance:
				if self.is_safe:
					self.is_safe = False
					rospy.loginfo("too close to an object robot is not safe")
				count +=1

		if count == 0 and not self.is_safe:
			self.is_safe = True
	
			
	def set_cmd_vel(self, msg):
		# Pick off the first marker (in case there is more than one)
		try:
			marker = msg.markers[0]
			if not self.target_visible:
				rospy.loginfo("FOLLOWER is Tracking Target!")
			self.target_visible = True
		except:
			# If target is lost, stop the robot by slowing it incrementally
			self.move_cmd.linear.x /= 1.5
			self.move_cmd.angular.z /= 1.5
			
			if self.target_visible:
				rospy.loginfo("FOLLOWER LOST Target!")
			self.target_visible = False
			
			return
				
		# Get the displacement of the marker relative to the base
		target_offset_y = marker.pose.pose.position.y
		rospy.loginfo("target_offset_y"+str(target_offset_y))
		
		# Get the distance of the marker from the base
		target_offset_x = marker.pose.pose.position.x
		rospy.loginfo("target_offset_x"+str(target_offset_x))
		
		# Rotate the robot only if the displacement of the target exceeds the threshold
		if abs(target_offset_y) > self.y_threshold:
			# Set the rotation speed proportional to the displacement of the target
			speed = target_offset_y * self.y_scale
			self.move_cmd.angular.z = copysign(max(self.min_angular_speed,
										min(self.max_angular_speed, abs(speed))), speed)
		else:
			self.move_cmd.angular.z = 0.0
 
		# Now get the linear speed
		if abs(target_offset_x - self.goal_x) > self.x_threshold:
			speed = (target_offset_x - self.goal_x) * self.x_scale
			if speed < 0:
				speed *= 1.5
			self.move_cmd.linear.x = copysign(min(self.max_linear_speed, max(self.min_linear_speed, abs(speed))), speed)
		else:
			self.move_cmd.linear.x = 0.0

	def shutdown(self):
		rospy.loginfo("Stopping the robot...")
		self.cmd_vel_pub.publish(Twist())
		rospy.sleep(1)	 

def main():
	tracker = TrackerBot()


main()
