#!/usr/bin/env python2.7

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

import rospy,  tf
import rosservice
from std_msgs.msg import Bool
from oscillation_ctrl.srv import UseCtrl, UseCtrlResponse 

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.use_ctrl = None
		self.ctrl_state = UseCtrlResponse()

		for i in range(3):
			try:
				self.use_ctrl = rospy.get_param('/status/change_mode')
			except:
				rospy.loginfo('Waiting for parameter "status/change_mode" to become available...')
				rospy.loginfo('Trying {0} more time(s)'.format(2-i))
				rospy.sleep(0.1)

		if self.use_ctrl is None:
			raise rospy.ROSInitException

		# service(s)
		rospy.Service('/status/att_ctrl_srv', UseCtrl, self.ctrl_tracker)

		# subscriber(s)	
		
		# publisher(s)
		self.state_pub = rospy.Publisher('/status/att_ctrl', Bool, queue_size=10)
		rospy.Timer(rospy.Duration(1.0/2.0), self.publisher)

	def ctrl_tracker(self, request):
		""" Keeps track of controller state """
		# check if we want to change state or whether it is a check
		if request.query:
			# change use_ctrl param to requested state
			rospy.set_param('/status/use_ctrl', request.state)

			# set result to new state
			self.ctrl_state.result = rospy.get_param('/status/use_ctrl')

			# check if everything worked
			if request.state is self.ctrl_state.result:
				self.ctrl_state.success = True
			else:
				self.ctrl_state.success = False
		else:
			self.ctrl_state.success = True
		return self.ctrl_state

	def publisher(self, timer):
		""" publish state so that nodes do not have query the service every time"""
		self.state_pub.publish(self.ctrl_state.result)

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