#!/usr/bin/env python2.7

### Cesar Rodriguez March 2022
### Reads waypoints from .yaml file and keeps track of them

import rospy,  tf
import rosservice
import time
from geometry_msgs.msg import Point
from oscillation_ctrl.srv import WaypointTrack, WaypointTrackResponse

class Main:

	def __init__(self):

		# Variables needed for testing start 
		self.tstart		= rospy.get_time() # Keep track of the start time
		while self.tstart == 0.0:				 # Need to make sure get_rostime works
			self.tstart		= rospy.get_time()
	
		# initialize variables
		self.xd = Point() 			# used to in response
		self.wpoints = []				# used to save waypoint from yaml file
		self.resp = WaypointTrackResponse() 
		self.param_exists = False # check in case param does not exist

		while self.param_exists == False:
			if rospy.has_param('status/waypoints'):
				self.wpoints	= rospy.get_param('status/waypoints') # get waypoints
				self.xd.x = self.wpoints['x']
				self.xd.y = self.wpoints['y']
				self.xd.z = self.wpoints['z']
				self.param_exists = True
			elif rospy.get_time() - self.tstart >= 5.0:
				self.xd.x = 0.0
				self.xd.y = 0.0
				self.xd.z = 1.5
				break

		# service(s)


		# subscriber(s)	
		# rospy.Subscriber('/reference/waypoints',Point, self.waypoints_cb)		
		
		# publisher(s)

	def waypoint_relay(self,req):
		#TODO Need to add check to make sure query is boolean
		if req.query:
			self.xd = req.xd
			self.resp.xd = self.xd
		else:
			self.resp.xd = self.xd
		return self.resp

	# Change desired position if there is a change
	# def waypoints_cb(self,msg):
	# 	try:
	# 		self.xd = msg
	# 	except ValueError or TypeError:
	# 		pass

if __name__=="__main__":

	# Initiate ROS node
	rospy.init_node('waypoints_server',anonymous=False)
	try:
		obj = Main() # create class object
		s = rospy.Service('status/waypoint_tracker',WaypointTrack,obj.waypoint_relay)
		rospy.loginfo_once('waypoints_server has started with:\nxd.x = %.1f\nxd.y = %.1f\nxd.z = %.1f', obj.xd.x, obj.xd.y, obj.xd.z)
			
		rospy.spin() # loop until shutdown signal

	except rospy.ROSInterruptException:
	        pass