#!/usr/bin/env python
import sys
import rospy

from sensor_msgs.msg import Joy
from std_msgs.msg import Header


#
# Manual input control helper, fakes joystick input
# > needs to correspond to default mapping in manual_input node
#
class ManualInput:

	def __init__(self):
		rospy.init_node('test_node', anonymous=True)
		self.joyPx4 = rospy.Publisher('px4_multicopter/joy', Joy, queue_size=10)
		self.joyIris = rospy.Publisher('iris/joy', Joy, queue_size=10)

	def arm(self):
		rate = rospy.Rate(10) # 10hz

		msg = Joy()
		msg.header = Header()
		msg.buttons = [0, 0, 0, 0, 0]
		msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] 
		count = 0
		while not rospy.is_shutdown() and count < 10:
			rospy.loginfo("zeroing")
			self.joyPx4.publish(msg)
			self.joyIris.publish(msg)
			rate.sleep()
			count = count + 1 

		msg.buttons = [0, 0, 0, 0, 0]
		msg.axes = [-1.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] 
		count = 0
		while not rospy.is_shutdown() and count < 10:
			rospy.loginfo("arming")
			self.joyPx4.publish(msg)
			self.joyIris.publish(msg)
			rate.sleep()
			count = count + 1

	def posctl(self):
		rate = rospy.Rate(10) # 10hz

		# triggers posctl
		msg = Joy()
		msg.header = Header()
		msg.buttons = [0, 0, 1, 0, 0]
		msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] 
		count = 0
		while not rospy.is_shutdown() and count < 10:
			rospy.loginfo("triggering posctl")
			self.joyPx4.publish(msg)
			self.joyIris.publish(msg)
			rate.sleep()
			count = count + 1